#include "pcloud_scans/get_accessible_area.h"
#include "nav_msgs/OccupancyGrid.h"
#define HIGHEST_OBS 2
#define ARCTAN18 0.324 
#define ARCTAN80 5.671
#define LINETOLINE_DIS 4
using namespace std;
using namespace visualization_msgs;
double thrd_K = 10;
double thrd_B = 0.15;

vector<Point> ext_points;
//通过直线找可通行区域
//输入 特征直线段
//返回0失败   返回1成功
void lines_accessible_area(Kmeans_class * Last_class,int K_means, int Ground_line, std::vector<pcl::PointCloud<pcl::PointXYZI> >laserCloudScansOrder)//滤波后的点云0-7
{
    //  ROS_INFO("lines_accessible_area");
    //viwer in rviz 
    visualization_msgs::Marker l_marker;
    l_marker.action = Marker::ADD;
    l_marker.color.a = 1.0;
    l_marker.color.r = 1.0f;
    l_marker.color.g = 1.0f;
    l_marker.color.b = 1.0f;
    l_marker.header.frame_id = "velodyne";
    l_marker.header.stamp = ros::Time::now();
    l_marker.id = 1;
    l_marker.ns = "k_lines";
    l_marker.scale.x = 0.15;
    l_marker.scale.y = 0.15;
    l_marker.scale.z = 0.15;
    l_marker.type = Marker::LINE_LIST;

    //viwer in rviz
    /* 
    for(int i=0; i<K_means; i++)
    {
        if(Last_class[i].Lines_num <= 1) 
            continue;
        printf("Class:  %d\n",Last_class[i].Class);
        printf("Lines_num:  %d\n",Last_class[i].Lines_num);
        for(int j=0; j<Ground_line; j++)
        {
            printf("    Line_num:  %d\n",(int)Last_class[i].detial[j].size());
            for(int k=0;k<Last_class[i].detial[j].size(); k++)
            {
                geometry_msgs::Point p;
                p.x = Last_class[i].detial[j][k].Start_point.x;
                p.y = Last_class[i].detial[j][k].Start_point.y;
                p.z = 0.1;
                l_marker.points.push_back(p);
                p.x = Last_class[i].detial[j][k].End_point.x;
                p.y = Last_class[i].detial[j][k].End_point.y;
                p.z = 0.2;
                l_marker.points.push_back(p);
                //printf("        Id_16lines:  %d\n",Last_class[i].detial[j][k].Id_16lines);
                //printf("        Id_alllines:  %d\n",Last_class[i].detial[j][k].Id_alllines);
                //printf("        K_b:  %f    %f\n",Last_class[i].detial[j][k].K_k,Last_class[i].detial[j][k].K_b);            
                //printf("        Start_point_set_x_y:  %d  %f  %f\n",Last_class[i].detial[j][k].Start_point.SET,Last_class[i].detial[j][k].Start_point.x, Last_class[i].detial[j][k].Start_point.y);       
                //printf("        End_point_set_x_y:    %d  %f  %f\n",Last_class[i].detial[j][k].End_point.SET,Last_class[i].detial[j][k].End_point.x, Last_class[i].detial[j][k].End_point.y);       
            }
        }
        printf("\n\n\n");
    }*/
   /* 
    for(int i=0; i<K_means; i++)
    {
        if(Last_class[i].Lines_num == 1) 
            continue;
        printf("Class:  %d\n",Last_class[i].Class);
        printf("Lines_num:  %d\n",Last_class[i].Lines_num);
        for(int j=0; j<Ground_line; j++)
        {
            printf("    Line_num:  %d\n",(int)Last_class[i].detial[j].size());
            for(int k=0;k<Last_class[i].detial[j].size(); k++)
            {
                geometry_msgs::Point p;
                p.x = Last_class[i].detial[j][k].Start_point.x;
                p.y = Last_class[i].detial[j][k].Start_point.y;
                p.z = 0.1;
                l_marker.points.push_back(p);
                p.x = Last_class[i].detial[j][k].End_point.x;
                p.y = Last_class[i].detial[j][k].End_point.y;
                p.z = 0.2;
                l_marker.points.push_back(p);
            }
        }
        printf("\n\n\n");
    }*/

    //寻找直线的端点、角度范围
   /* std::vector<std::vector<double> > points_ang_results;
    for(int i=0; i<K_means; i++)   //所有端点的距离最长的端点（首尾端点就好）
    {
        if(Last_class[i].Lines_num<2)
            continue;
        //找到障碍物角度范围，计算斜率和截距
        std::vector<Point> points_ang_sor;
        double Ave_k = 0.0;
        double Ave_b = 0.0;
        Point temp_point;
        for(int m=0; m<Ground_line; m++)
        {
            for(int n=0; n<Last_class[i].detial[m].size(); n++)
            {
                temp_point.x = Last_class[i].detial[m][n].Start_point.x;
                temp_point.y = Last_class[i].detial[m][n].Start_point.y;
                points_ang_sor.push_back(temp_point);
                //printf("x_y:    %f  %f   \n",points_ang_sor[points_ang_sor.size()-1].x,points_ang_sor[points_ang_sor.size()-1].y);  
                temp_point.x = Last_class[i].detial[m][n].End_point.x;
                temp_point.y = Last_class[i].detial[m][n].End_point.y;
                points_ang_sor.push_back(temp_point);
                Ave_k = Ave_k + Last_class[i].detial[m][n].K_k;
                Ave_b = Ave_b + Last_class[i].detial[m][n].K_b;
                //printf("x_y:    %f  %f   \n",points_ang_sor[points_ang_sor.size()-1].x,points_ang_sor[points_ang_sor.size()-1].y);  
            }
        }

        Point thold_start;
        Point thold_end;
        double thold_dis = 0;
        for(int m=0; m<points_ang_sor.size(); m=m+2)//首点
        {
            for(int n=1; n<points_ang_sor.size(); n=n+2)//对应  尾点
            {
                //printf("x_y:    %f  %f    %f  %f\n",points_ang_sor[m].x,points_ang_sor[m].y, points_ang_sor[n].x,points_ang_sor[n].y);  
                double temp_x = (points_ang_sor[m].x -  points_ang_sor[n].x)*(points_ang_sor[m].x -  points_ang_sor[n].x);
                double temp_y = (points_ang_sor[m].y -  points_ang_sor[n].y)*(points_ang_sor[m].y -  points_ang_sor[n].y);
                double temp_dis = sqrt(temp_x + temp_y);
                if(temp_dis < thold_dis)
                    continue;
                thold_dis = temp_dis;
                thold_start.x = points_ang_sor[m].x;
                thold_start.y = points_ang_sor[m].y;
                thold_end.x = points_ang_sor[n].x;
                thold_end.y = points_ang_sor[n].y;
            }
        }
        //printf("x_y:    %f  %f    %f  %f\n",thold_start.x,thold_start.y, thold_end.x,thold_end.y);  
        Ave_k = Ave_k*2/points_ang_sor.size();
        Ave_b = Ave_b*2/points_ang_sor.size();
        double Start_ang = std::atan2(thold_start.x,thold_start.y)*(180/M_PI);
        if(Start_ang<0.0) 
            Start_ang = Start_ang + 360;
        double End_ang = std::atan2(thold_end.x,thold_end.y)*(180/M_PI);
        if(End_ang<0.0) 
            End_ang = End_ang + 360;
        vector<double> temp_points_ang;
        temp_points_ang.push_back(Start_ang);
        temp_points_ang.push_back(End_ang);    
        temp_points_ang.push_back(Ave_k); 
        temp_points_ang.push_back(Ave_b);
        temp_points_ang.push_back(thold_start.x);//保证数据隔离，没啥问题
        temp_points_ang.push_back(thold_start.y);
        temp_points_ang.push_back(thold_end.x);
        temp_points_ang.push_back(thold_end.y);
        points_ang_results.push_back(temp_points_ang);    
        printf("End_point_set_x_y: start  end   %f  %f   avek aveb   %f  %f\n",Start_ang, End_ang, Ave_k, Ave_b);  
    }*/
    //重新检测b=36部分线段 求在x轴上的截距
    for(int i=0; i<K_means ; i++)  
    {
        if(Last_class[i].Lines_num<2)//0/1 直接丢弃
            continue;
        double Ave_k = 0;
        double Ave_b = 0;
        for(int m=0; m<Ground_line; m++)//对于每一条扫描线
        {
            for(int n=0; n<Last_class[i].detial[m].size(); n++)//对于每一条扫描线上的 线段
            {
                double x1 = Last_class[i].detial[m][n].Start_point.x, y1 = Last_class[i].detial[m][n].Start_point.y;
                double x2 = Last_class[i].detial[m][n].End_point.x, y2 = Last_class[i].detial[m][n].End_point.y;  
                double temp_K = std::atan2(x1-x2, y1-y2) * (180/M_PI);
                if(temp_K<0.0) temp_K = temp_K + 360;
                double temp_B;
                if(fabs(temp_K-90)<1e-6 || fabs(temp_K-270)<1e-6)  
                {
                    temp_B = y1;//垂直
                }
                else if(temp_K<1e-6 || 360-temp_K<1e-6 || fabs(temp_K-180)<1e-6)  
                {
                    temp_B = 36.0;//水平
                }
                else   
                {
                    temp_B = y1 - x1/std::tan(temp_K*M_PI/180);//输入的是弧度 
                    if(temp_B>36) temp_B = 36;
                    if(temp_B<-36) temp_B = -36;
                }
                Ave_k = Ave_k+fabs(temp_K);//可以不用管了？
                Ave_b = Ave_b+fabs(temp_B);    //-36/36徘徊
            }
        }
        Ave_k = Ave_k/Last_class[i].Lines_num;
        Ave_b = Ave_b/Last_class[i].Lines_num;  
        if(Ave_b<25)
            continue;
        //计算在x轴上的平均截距
        //减去差值最大的一个temp_B，直到Ave_yb和temp_yB差值0.3m的范围/只剩下最后一根直线段，就可以把不平行的丢弃了
        double Ave_kx = 0;
        double Ave_bx = 0;
        vector<vector<double> >temp_kb;
        for(int m=0; m<Ground_line; m++)//对于每一条扫描线
        {
            for(int n=0; n<Last_class[i].detial[m].size(); n++)//对于每一条扫描线上的 线段
            {
                double x1 = Last_class[i].detial[m][n].Start_point.x, y1 = Last_class[i].detial[m][n].Start_point.y;
                double x2 = Last_class[i].detial[m][n].End_point.x, y2 = Last_class[i].detial[m][n].End_point.y;  
                double temp_K = std::atan2(x1-x2, y1-y2) * (180/M_PI);
                if(temp_K<0.0) temp_K = temp_K + 360;
                double temp_B;
                if(fabs(temp_K-90)<1e-6 ||fabs(temp_K-270)<1e-6)  
                {
                    temp_B = 36;//垂直
                }
                else if(temp_K<1e-6 || 360-temp_K<1e-6  || fabs(temp_K-180)<1e-6)  
                {
                    temp_B = x1;//水平
                }
                else   
                {
                    temp_B = x1 - y1*std::tan(temp_K*M_PI/180);//输入的是弧度 
                    if(temp_B>36) temp_B = 36;
                    if(temp_B<-36) temp_B = -36;
                }
                vector<double> temp;
                temp.push_back(temp_B);
                temp.push_back(temp_K);
                temp.push_back(m);
                temp.push_back(n);
                temp_kb.push_back(temp);
                Ave_bx = Ave_bx + temp_B;
                Ave_kx = Ave_kx + temp_K;
            }
        }
        sort(temp_kb.begin(),temp_kb.end());
        while(temp_kb.size()>1)
        {
            double temp_Ave_bx = Ave_bx/temp_kb.size();//均值
            //printf("temp_Ave_bx:   %f\n",temp_Ave_bx);/////////////////////////////////////////////////////////
            vector<vector<double> >temp_db;
            for(int m=0; m<temp_kb.size(); m++)//b和均值的差值
            {
                vector<double> temp;
                temp.push_back(fabs(temp_kb[m][0] - temp_Ave_bx));
                temp.push_back(m);
                temp_db.push_back(temp);
                //printf("scanid db:   %d  %f\n",m,fabs(temp_kb[m][0] - temp_Ave_bx));/////////////////////////////////////////////////////////
            }
            sort(temp_db.begin(),temp_db.end());//从小到大
            if(temp_db[temp_db.size()-1][0] < 0.25)
                break;
            int dpmax_index = temp_db[temp_db.size()-1][1];
            int m = temp_kb[dpmax_index][2];
            int n = temp_kb[dpmax_index][3];
            Ave_bx = Ave_bx - temp_kb[dpmax_index][0]; //Ave_bx - maxdb
            temp_kb.erase(temp_kb.begin() + dpmax_index);
            Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+n);
            Last_class[i].Lines_num--;
            //printf("sLast_class[i]:  %d   %d  %d\n",dpmax_index,m,n);/////////////////////////////////////////////////////////
        }

    }

    //找到直线段之间缺省的直线
    pcl::PointCloud<pcl::PointXYZI>::Ptr Line_ptr (new pcl::PointCloud<pcl::PointXYZI>);
    for(int i=0; i<K_means ; i++)   //分析每一个聚类
    {
        //printf("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
        if(Last_class[i].Lines_num<2)//0/1 直接丢弃
            continue;

        //先把直线附近的点//起始点到终止点
        pcl::PointXYZI point;
        std::vector<std::vector<double> > Point_set;
        for(int m=0; m<Ground_line; m++)//对于每一条扫描线
        {
            for(int n=0; n<Last_class[i].detial[m].size(); n++)//对于每一条扫描线上的 线段
            {
                double x1 = Last_class[i].detial[m][n].Start_point.x, y1 = Last_class[i].detial[m][n].Start_point.y;
                double x2 = Last_class[i].detial[m][n].End_point.x, y2 = Last_class[i].detial[m][n].End_point.y;
                double c = pow(y2-y1,2) + pow(x1-x2,2);
                int h = Last_class[i].detial[m][n].Start_point.SET;//-35 > 0 ? Last_class[i].detial[m][n].Start_point.SET-35:0;
                int y = Last_class[i].detial[m][n].End_point.SET;//+35 > (int)laserCloudScansOrder[m].size() ? \
                                                                 //        (int)laserCloudScansOrder[m].size():Last_class[i].detial[m][n].End_point.SET+35;
                for(int k=h; k<y; k++)//（）
                {
                    double a = (laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x-x1) * (y2-y1);
                    double b = (laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y-y1) * (x1-x2);
                    double dis = sqrt((a+b)*(a+b)/c);//点到直线的距离
                    if(dis>0.05)
                        continue;
/*                    point.x = laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x;
                    point.y = laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y;
                    point.z = laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].z;
                    point.intensity = laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].intensity;
                    Line_ptr->push_back(point);
*/
                    std::vector<double> temp_point_mn;
                    temp_point_mn.push_back(sqrt(pow(laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x,2) + \
                                                              pow(laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y,2)));//直线kb上所有点到原点的距离
                    temp_point_mn.push_back(laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x);
                    temp_point_mn.push_back(laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y);
                    temp_point_mn.push_back(m);
                    temp_point_mn.push_back(n);
                    Point_set.push_back(temp_point_mn);
                } 
            }
        }
        if(Point_set.size()<2)
            continue;
        //对于直线上所有的点，判断点的间距，间距很大并且这两个点不是相邻扫描线上的点，则该位置缺失，
        //距离这两个点最近的线段的端点  组成的直线k/b， 这两个端点所在扫面线的中间就是缺失的线段
        //1.能否组成线段（近距离的点组成直线的k/b是否近似）  能，障碍物      
        //              不能，射线是否穿越   否，不操作
        //                                是，单个线段的都删除，
        //把直线附近的点填充，即重复以上步骤
        //默认第一列排序、 由小到大, 找到端点（其实不用排序，只要最大的，优化）
        std::sort(Point_set.begin(),Point_set.end());
        //距离最远的一定是直线kb的端点
        double Start_x = Point_set[Point_set.size()-1][1];
        double Start_y = Point_set[Point_set.size()-1][2];
        //kb上的点到端点的距离
        for(int m=0; m<Point_set.size(); m++)
        {
            double dis = pow(Point_set[m][1] - Start_x,2) + pow(Point_set[m][2] - Start_y,2);
            Point_set[m][0] = sqrt(dis);
        } 
        //默认第一列排序、 由小到大, 排序（假设kb确定是直线,理论上角度更合适，不过复杂点）
        std::sort(Point_set.begin(),Point_set.end());
        //点的序列，由小到大
        for(int m=0; m<Point_set.size()-1; m++)
        {
            double dis_euclidean = fabs(Point_set[m][0] - Point_set[m+1][0]);//表示连续点之间的距离
            int dis_lines = abs(Point_set[m][3] - Point_set[m+1][3]);//表示点所在的扫描线之间的距离   
            //扫描线间距必须大于等于1     
            if(dis_euclidean<0.25 || dis_lines<2 || dis_lines>4)
                continue;
            //扫描线中缺省直线段位置
            int tmp1 = (int)Point_set[m][3];//m
            int tmp2 = (int)Point_set[m][4];//n
            int tmp3 = (int)Point_set[m+1][3];//m+1
            int tmp4 = (int)Point_set[m+1][4];//n+1
            //首尾点距离
            double tempStart_dis = pow(Last_class[i].detial[tmp1][tmp2].Start_point.x - Last_class[i].detial[tmp3][tmp4].End_point.x,2) + \
                                       pow(Last_class[i].detial[tmp1][tmp2].Start_point.y - Last_class[i].detial[tmp3][tmp4].End_point.y,2);
            //尾首点距离
            double tempEnd_dis = pow(Last_class[i].detial[tmp1][tmp2].End_point.x - Last_class[i].detial[tmp3][tmp4].Start_point.x,2) +   \
                                       pow(Last_class[i].detial[tmp1][tmp2].End_point.y - Last_class[i].detial[tmp3][tmp4].Start_point.y,2);
            /////////////////////////////////////////////////////////
            //printf("ppppppx_y1: x:  %f  %f y:  %f  %f\n",Last_class[i].detial[tmp1][tmp2].Start_point.x, Last_class[i].detial[tmp1][tmp2].Start_point.y, \
                                                        Last_class[i].detial[tmp1][tmp2].End_point.x, Last_class[i].detial[tmp1][tmp2].End_point.y);  
            //printf("ppppppx_y2: x:  %f  %f y:  %f  %f\n",Last_class[i].detial[tmp3][tmp4].Start_point.x, Last_class[i].detial[tmp3][tmp4].Start_point.y, \
                                                        Last_class[i].detial[tmp3][tmp4].End_point.x, Last_class[i].detial[tmp3][tmp4].End_point.y);  
            /////////////////////////////////////////////////////////
            //距离较小的两个点作为kb
            double x1, y1, x2, y2;//x2尾点 -> x1首点
            int h,y;
            if(tempStart_dis > tempEnd_dis) 
            {
                x1 = Last_class[i].detial[tmp3][tmp4].Start_point.x, y1 = Last_class[i].detial[tmp3][tmp4].Start_point.y;
                x2 = Last_class[i].detial[tmp1][tmp2].End_point.x, y2 = Last_class[i].detial[tmp1][tmp2].End_point.y;
                h = Last_class[i].detial[tmp1][tmp2].End_point.SET;
                y = Last_class[i].detial[tmp3][tmp4].Start_point.SET;
            }
            else
            {
                x1 = Last_class[i].detial[tmp1][tmp2].Start_point.x, y1 = Last_class[i].detial[tmp1][tmp2].Start_point.y;
                x2 = Last_class[i].detial[tmp3][tmp4].End_point.x, y2 = Last_class[i].detial[tmp3][tmp4].End_point.y;
                h = Last_class[i].detial[tmp3][tmp4].End_point.SET;
                y = Last_class[i].detial[tmp1][tmp2].Start_point.SET;//换成角度才是正确的(比例不行，非线性缺省)！！！！！！！！！！！！！！！！！！！！！！！！！！！！
            }
                   
            double c = pow(y2-y1,2) + pow(x1-x2,2);
            //printf("dubeg1 ！！！！！！！！！！！！！   \n");
            //j扫面线的id 从小到大
            for(int j=min(tmp1,tmp3)+1; j<max(tmp1,tmp3); j++)
            {   
                //printf("dubeg2 ！！！！！！！！！！！！！   \n");
                vector <vector<double> > temp_points;
                //printf("scanid3: x:  %d   \n",j);/////////////////////////////////////////////////////////
                //printf("start-end1: %d  %d   \n",h,y);
                int temp_h = h-35 > 0 ? h-35:0;
                int temp_y = y+35 > (int)laserCloudScansOrder[j].size() ? (int)laserCloudScansOrder[j].size():y+35;
                //点到直线的距离
                if(temp_h < temp_y)//外线高于内线，近似平行，出现负角度
                {
                    //printf("left: start-end1: %d  %d   \n",temp_h,temp_y);
                    for(int k=temp_h; k<temp_y; k++)//右侧
                    {   
                        //printf("left:   %d   \n",k);
                        double a = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x-x1) * (y2-y1);
                        double b = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y-y1) * (x1-x2);
                        double dis = sqrt((a+b)*(a+b)/c);
                        if(dis>0.02)
                            continue;
                        vector<double> pcl_point;
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x);
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y);
                        pcl_point.push_back(k);//点在扫面线上的 移动 id
                        temp_points.push_back(pcl_point);

                        /*pcl::PointXYZI point;
                        point.x = laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x;
                        point.y = laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y;
                        point.z = laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].z;
                        Line_ptr->push_back(point);*/
                    }
                }
                if(temp_h >= temp_y)
                {
                    //printf("right:start-end1: %d  %d   \n",temp_h,temp_y);
                    for(int k=temp_h; k<laserCloudScansOrder[j].size(); k++)//右侧
                    {
                        double a = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x-x1) * (y2-y1);
                        double b = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y-y1) * (x1-x2);
                        double dis = sqrt((a+b)*(a+b)/c);
                        if(dis>0.02)
                            continue;
                        vector<double> pcl_point;
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x);
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y);
                        pcl_point.push_back(k);//点在扫面线上的 移动 id
                        temp_points.push_back(pcl_point);

                        /*pcl::PointXYZI point;
                        point.x = laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x;
                        point.y = laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y;
                        point.z = laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].z;
                        Line_ptr->push_back(point);*/
                    }
                    //printf("ok0 \n");
                    for(int k=0; k<temp_y; k++)
                    {
                        double a = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x-x1) * (y2-y1);
                        double b = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y-y1) * (x1-x2);
                        double dis = sqrt((a+b)*(a+b)/c);
                        if(dis>0.02)
                            continue;
                        vector<double> pcl_point;
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x);
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y);
                        pcl_point.push_back(k);//点在扫面线上的 移动 id
                        temp_points.push_back(pcl_point);

                        /*pcl::PointXYZI point;
                        point.x = laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x;
                        point.y = laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y;
                        point.z = laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].z;
                        Line_ptr->push_back(point);*/
                    }
                }
                //printf("dubeg4 ！！！！！！！！！！！！！   \n");
                //srvprintf("ok1 \n");
                if(temp_points.size()<10) 
                    continue;
                vector<vector<int> >index_temp_points;
                vector<int> temp_indes;
                temp_indes.push_back(0);
                for(int k=0; k<temp_points.size()-1; k++)
                {
                    temp_indes.push_back(k);
                    if(4 < fabs(temp_points[k][2] - temp_points[k+1][2]) || k >= temp_points.size()-2)//点间距大 （换成参数）
                    {
                        temp_indes.erase(temp_indes.begin());//删除第一个元素
                        index_temp_points.push_back(temp_indes);//存储有效id
                        temp_indes.push_back(0);//随意添加一个元素
                        temp_indes.erase(temp_indes.begin()+1, temp_indes.end());//只留下一个
                    }
                }
                //for(int k=0; k<index_temp_points.size(); k++)
                //{
                //     printf("index_temp_points size %d\n",(int)index_temp_points[k].size());
                //}
                //printf("OK!!!!!!!!!\n");
                //分析这一扫描线上的 缺省线段的kb
                //斜率\截距\相关系数
                //函数：x = Ky + B  其中 K = d1x/d1y; B = x1-Ky1;  当x=0时的 截距 b = -b/k = y1 - x1/K
                double temp_K = std::atan2(x1-x2, y1-y2) * (180/M_PI);
                if(temp_K<0.0) temp_K = temp_K + 360;
                double temp_B;
                if(fabs(temp_K-90)<1e-6 || fabs(temp_K-270)<1e-6)  
                {
                    temp_B = y1;//垂直
                }
                else if(temp_K<1e-6 || 360-temp_K<1e-6 || fabs(temp_K-180)<1e-6)  
                {
                    temp_B = 36.0;//水平
                }
                else   
                {
                    temp_B = y1 - x1/std::tan(temp_K*M_PI/180);//输入的是弧度 
                    if(temp_B>36) temp_B = 36;
                    if(temp_B<-36) temp_B = -36;
                }
                //printf("LineFitLeastSquares  temp_K: %f   temp_B: %f %f %f %f %f \n",temp_K,temp_B,x1,y1,x2,y2);/////////////////////////////////////////////////////////
                temp_K = std::atan((x1-x2)/(y1-y2)) * (180/M_PI);
                //printf("LineFitLeastSquares  temp_K: %f   \n",temp_K);

                //把近点  最小二乘拟合，根据kbr选择有效的缺省直线段
                for(int k=0; k<index_temp_points.size(); k++)
                {
                    if(10 > index_temp_points[k].size())//点的数目不够（换成参数）
                        continue;
                    double temp_dis = pow(temp_points[index_temp_points[k][0]][0] - temp_points[index_temp_points[k][index_temp_points[k].size()-1]][0],2) + \
                                      pow(temp_points[index_temp_points[k][0]][1] - temp_points[index_temp_points[k][index_temp_points[k].size()-1]][1],2);
                    if(0.25 > sqrt(temp_dis))//线段长度不够（换成参数）
                        continue;

                    vector<double> return_KB;
                    LineFitLeastSquares(index_temp_points[k], temp_points, return_KB);
                    if(fabs(return_KB[0]-temp_K)>thrd_K+1 || fabs(return_KB[1]-temp_B)>thrd_B+0.05)
                        continue;

                    //if(! LineFitLeastSquares(index_temp_points[k], temp_points, temp_K, temp_B, thrd_K, thrd_B))//kbr筛选
                    //    continue;
                    /*for(int p=0; p<index_temp_points[k].size(); p++)
                    {
                        point.x = temp_points[index_temp_points[k][p]][0];
                        point.y = temp_points[index_temp_points[k][p]][1];
                        point.z = 0;
                        Line_ptr->push_back(point);
                    }*/
                    Kmeans_line temp_point;//RESIZE VECTOR
                    Last_class[i].Lines_num++;
                    temp_point.Id_16lines = j;					 //该条线的线的ID
                    //temp_point.Id_alllines = 0;				 //该条线总数目ID
                    temp_point.K_k = return_KB[0];
                    temp_point.K_b = return_KB[1];
                    temp_point.Start_point.x =temp_points[*index_temp_points[k].begin()][0];//该线段首尾点的信息
                    temp_point.Start_point.y =temp_points[*index_temp_points[k].begin()][1];//该线段首尾点的信息
                    temp_point.Start_point.SET =temp_points[*index_temp_points[k].begin()][2];//该线段首尾点的信息
                    temp_point.End_point.x =temp_points[*(index_temp_points[k].end()-1)][0];//该线段首尾点的信息
                    temp_point.End_point.y =temp_points[*(index_temp_points[k].end()-1)][1];//该线段首尾点的信息
                    temp_point.End_point.SET =temp_points[*(index_temp_points[k].end()-1)][2];//该线段首尾点的信息
                    Last_class[i].detial[j].push_back(temp_point);//有问题，在这里需要剔除重复的
                    //printf("dubeg5 ！！！！！！！！！！！！！   \n");
                }
            }
        }
        //printf("最后的丢弃1 ！！！！！！！！！！！！！   \n");
    }


    //判断内侧四根相邻或相间扫描线上的直线段之间的端点距离（最近）不能超过2.5米，如果超过2.5米（因为水平内侧四条线的半径间距不超过三米），丢掉内侧的、、防止下坡的问题,
    //反向怎么办？
   /* for(int i=0; i<K_means ; i++)   //分析每一个聚类
    {

        if(Last_class[i].Lines_num<2)//0/1 直接丢弃
            continue;
        for(int m=0; m<3; m++)//对于每一条扫描线 012
        {  
            if(Last_class[i].detial[m].size()>2)
                continue;
            if(Last_class[i].detial[m+1].size()>2)
                continue;
            if(0 == Last_class[i].detial[m].size())
                continue;
            if(0 == Last_class[i].detial[m+1].size() && 0 == Last_class[i].detial[m+2].size())
                continue;

            vector<vector<double> > pose1;//扫描线m上的端点
            for(int n=0; n<=Last_class[i].detial[m].size(); n++)
            {  
                //如果有一条直线段长度比较长，就直接跳过
                double temp_dis = pow(Last_class[i].detial[m][n].Start_point.x - Last_class[i].detial[m][n].End_point.x, 2) +\
                                            pow(Last_class[i].detial[m][n].Start_point.y - Last_class[i].detial[m][n].End_point.y, 2);
                if(sqrt(temp_dis)>1.5) 
                    break;
                vector<double> temp;
                temp.push_back(m);
                temp.push_back(Last_class[i].detial[m][n].Start_point.x);
                temp.push_back(Last_class[i].detial[m][n].Start_point.y);
                pose1.push_back(temp);
                vector<double> temp1;
                temp1.push_back(m);
                temp1.push_back(Last_class[i].detial[m][n].End_point.x);
                temp1.push_back(Last_class[i].detial[m][n].End_point.y);
                pose1.push_back(temp1);
            }

            vector<vector<double> > pose2;//扫描线temp_m上的端点
            int temp_m = m+1;
            if(Last_class[i].detial[temp_m].size()==0)
            {
                 temp_m++;
            }
            if(temp_m>3 || Last_class[i].detial[temp_m].size()>2)
                break;
            for(int n=0; n<=Last_class[i].detial[temp_m].size(); n++)//对于每一条扫描线 01 02/ 12 13/ 23
            {  
                //如果有一条直线段长度比较长，就直接跳过
                double temp_dis = pow(Last_class[i].detial[temp_m][n].Start_point.x - Last_class[i].detial[temp_m][n].End_point.x, 2) +\
                                    pow(Last_class[i].detial[temp_m][n].Start_point.y - Last_class[i].detial[temp_m][n].End_point.y, 2);
                if(sqrt(temp_dis)>1.5) 
                    break;
                vector<double> temp;
                temp.push_back(temp_m);
                temp.push_back(Last_class[i].detial[temp_m][n].Start_point.x);
                temp.push_back(Last_class[i].detial[temp_m][n].Start_point.y);
                pose2.push_back(temp);
                vector<double> temp1;
                temp1.push_back(temp_m);
                temp1.push_back(Last_class[i].detial[temp_m][n].End_point.x);
                temp1.push_back(Last_class[i].detial[temp_m][n].End_point.y);
                pose2.push_back(temp1);
            }
            printf("sssssssssssssssssssssss\n");
            for(int j=0; j<pose1.size(); j++)
            {
                for(int k=0; k<pose2.size(); k++)
                {
                    double temp_dis = pow(pose1[j][1] - pose2[k][1], 2) + pow(pose1[j][2] - pose2[k][2], 2);
                    if(sqrt(temp_dis)>4.5) 
                    {   
                        printf("333333333333333333333:%f,%f,%f,%f\n",pose1[j][1] ,pose1[j][2], pose2[k][1],pose2[k][2]);
                        Last_class[i].detial[m].erase(Last_class[i].detial[m].begin(), Last_class[i].detial[m].end());
                        Last_class[i].Lines_num -= Last_class[i].detial[m].size();
                        m--;
                        k = 1000;
                        j = 1000;
                    }
                }
            }
        }
    }*/
    
    //printf("kkkkkkkkkkkkkkkkkkkk\n");/////////////////////////////////////////////////////////
    for(int i=0; i<K_means ; i++)   //分析每一个聚类
    {
        //printf("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
        if(Last_class[i].Lines_num<2)//0/1 直接丢弃
            continue;
        //有问题线段 总是在一个聚类中扫描线id最小的一个上，偶然性，所以只有一扫描线上的有问题，找到条扫描线上的所有直线段，
        //通常体现：一根（也有可能两根），长度短（也可能长），相邻扫面线上没有直线段（也可能有），如果有，另一条扫面线连线的kb，缺省扫面线在kb近处的点很少/近点组成的直线kb相差很大
        vector<vector<int> > num_lines;
        for(int m=0; m<Ground_line; m++)//对于每一条扫描线
        {   
            if(0 == Last_class[i].detial[m].size())
                continue;
            vector<int> temp_order;
            temp_order.push_back(m);
            temp_order.push_back(Last_class[i].detial[m].size());

            num_lines.push_back(temp_order);            
        }
        if(num_lines[0][1]>=3)
            continue;
        //000n01这样的直接分开 n<3
        if(num_lines.size()==2 && num_lines[1][0]-num_lines[0][0]>1 && num_lines[1][1]<=2 && num_lines[0][1]==1)   
        {
            int temp_scanid = num_lines[0][0];
            Last_class[i].detial[temp_scanid].erase(Last_class[i].detial[temp_scanid].begin(), Last_class[i].detial[temp_scanid].end());
            Last_class[i].Lines_num -= Last_class[i].detial[temp_scanid].size();
            temp_scanid = num_lines[1][0];
            Last_class[i].detial[temp_scanid].erase(Last_class[i].detial[temp_scanid].begin(), Last_class[i].detial[temp_scanid].end());
            Last_class[i].Lines_num--;
            //printf("Error 0\n");///////////////////////////////////////////////////////////	
        }//n001
        else if(num_lines.size()>=2 && num_lines[1][0]-num_lines[0][0]>2 && num_lines[0][1]==1)        
        {
            int temp_scanid = num_lines[0][0];
            Last_class[i].detial[temp_scanid].erase(Last_class[i].detial[temp_scanid].begin(), Last_class[i].detial[temp_scanid].end());
            Last_class[i].Lines_num--;
            //printf("Error 2\n");///////////////////////////////////////////////////////////	
        }
        //n0002这样的直接分开
        else if(num_lines.size()>=2 && num_lines[1][0]-num_lines[0][0]>3 && num_lines[0][1]==2)        
        {
            int temp_scanid = num_lines[0][0];
            Last_class[i].detial[temp_scanid].erase(Last_class[i].detial[temp_scanid].begin(), Last_class[i].detial[temp_scanid].end());
            Last_class[i].Lines_num -= Last_class[i].detial[temp_scanid].size();

            //printf("Error 3\n");///////////////////////////////////////////////////////////	
        }
        //n00011
        else if(num_lines.size()>=3 && num_lines[2][0]-num_lines[1][0]>3 && num_lines[1][0]-num_lines[0][0]==1 && num_lines[1][1]==1 && num_lines[0][1]==1)
        {
            int temp_scanid = num_lines[0][0];
            Last_class[i].detial[temp_scanid].erase(Last_class[i].detial[temp_scanid].begin(), Last_class[i].detial[temp_scanid].end());
            Last_class[i].Lines_num--;
            temp_scanid = num_lines[1][0];
            Last_class[i].detial[temp_scanid].erase(Last_class[i].detial[temp_scanid].begin(), Last_class[i].detial[temp_scanid].end());
            Last_class[i].Lines_num--;
            //printf("Error 4\n");///////////////////////////////////////////////////////////	
        }
        else
        {
           /* printf("1111111111111111111111111\n");
            for(int m=0; m<Ground_line-1; m++)//对于每一条扫描线
            {
                printf("MMMMM %d\n",m);
                if(Last_class[i].detial[m].size() == 0)
                    continue;
                if(Last_class[i].detial[m].size() > 2)
                    break;

                double temp_K=0, temp_B=0;
                for(int n=0; n<Last_class[i].detial[m].size(); n++)
                {
                    temp_K += Last_class[i].detial[m][n].K_k;
                    temp_B += Last_class[i].detial[m][n].K_b;
                }
                temp_K = temp_K/Last_class[i].detial[m].size();
                temp_B = temp_B/Last_class[i].detial[m].size();
                //找到相邻扫描线
                int temp_m = m+1;
                while(temp_m<Ground_line)
                {
                    if(Last_class[i].detial[temp_m].size() > 0)
                        break;
                    else temp_m++;
                }
                if(temp_m >= Ground_line)
                    break;
                //数据处理
                if(Last_class[i].detial[temp_m].size() > 2)
                {
                    printf("2222222222222222222222222\n");
                    //判断Dz大不大
                    std::vector<double> Point_hight;
                    for(int n=0; n<Last_class[i].detial[m].size(); n++)
                    {   
                        int start = Last_class[i].detial[m][n].Start_point.SET;
                        int end = Last_class[i].detial[m][n].End_point.SET;
                        for(int j=start; j<end; j++)
                        {
                            Point_hight.push_back(laserCloudScansOrder[m][j].z);
                        }
                    }
                    std::sort(Point_hight.begin(),Point_hight.end());
                    if(fabs(Point_hight[0] - Point_hight[Point_hight.size()-1]) > 0.04)
                    {
                        printf("333333333333333333333333\n");
                        break;
                    }
                    else
                    {
                        //大，丢弃  
                        Last_class[i].detial[m].erase(Last_class[i].detial[m].begin(),Last_class[i].detial[m].end());
                        Last_class[i].Lines_num = Last_class[i].Lines_num - Last_class[i].detial[m].size();
                        printf("444444444444444444444444444\n");
                        break;
                    }
                }
                else
                {
                    printf("5555555555555555555555555\n");
                    double temp_KK=0, temp_BB=0;
                    for(int n=0; n<Last_class[i].detial[temp_m].size(); n++)//Last_class[i].detial[temp_m].size()必定大于0
                    {
                        temp_KK += Last_class[i].detial[temp_m][n].K_k;
                        temp_BB += Last_class[i].detial[temp_m][n].K_b;
                    }
                    //判断kb差距大不大
                    temp_KK = temp_KK/Last_class[i].detial[temp_m].size();
                    temp_BB = temp_BB/Last_class[i].detial[temp_m].size();

                    printf("66666666666666666666666666666\n");
                    //大，丢弃
                    if(fabs(temp_KK - temp_K)> 6 || fabs(temp_BB - temp_B > 2))
                    {
                        Last_class[i].detial[m].erase(Last_class[i].detial[m].begin(),Last_class[i].detial[m].end());
                        Last_class[i].Lines_num = Last_class[i].Lines_num - Last_class[i].detial[m].size();
                        printf("7777777777777777777777777777\n");
                       continue;
                    }
                    //小，//判断Dz大不大
                    else
                    {
                        printf("88888888888888888888888888\n");
                        //判断Dz大不大
                        std::vector<double> Point_hight;
                        for(int n=0; n<(int)Last_class[i].detial[m].size(); n++)
                        {   
                            int start = Last_class[i].detial[m][n].Start_point.SET;
                            int end = Last_class[i].detial[m][n].End_point.SET;
                            for(int j=start; j<end; j++)
                            {
                                Point_hight.push_back(laserCloudScansOrder[m][j].z);
                            }
                        }
                        std::sort(Point_hight.begin(),Point_hight.end());
                        //不大，保留 //结束本次测试
                        if(fabs(Point_hight[0] - Point_hight[Point_hight.size()-1]) > 0.02)//有问题！！！！！！！！！！！！！！！！
                        {
                            printf("9999999999999999999999999999\n");
                            break;
                        }
                        else
                        {
                            //大，丢弃  
                            Last_class[i].detial[m].erase(Last_class[i].detial[m].begin(),Last_class[i].detial[m].end());
                            Last_class[i].Lines_num = Last_class[i].Lines_num - Last_class[i].detial[m].size();
                            printf("bbbbbbbbbbbbbbbbbbbbbbbb\n");
                            continue;
                        }
                    }
                }
            }*/
        } 
    }
    //printf("aaaaaaaaaaaaaaaaa\n");///////////////////////////////////////////////////////////	

    //因为丢弃了一些线段，需要再次填充
    for(int i=0; i<K_means ; i++)   //分析每一个聚类
    {
        if(Last_class[i].Lines_num<2)//0/1 直接丢弃
            continue;
        //printf("OK1!!!!!!!!!\n");/////////////////////////////////////////////////////////
        //找到直线附近的点
        pcl::PointXYZI point;
        //检测每一条线
        std::vector<std::vector<double> > Point_set;
        for(int m=0; m<Ground_line; m++)//对于每一条扫描线
        {
            for(int n=0; n<Last_class[i].detial[m].size(); n++)//对于每一条扫描线上的 线段
            {
                double x1 = Last_class[i].detial[m][n].Start_point.x, y1 = Last_class[i].detial[m][n].Start_point.y;
                double x2 = Last_class[i].detial[m][n].End_point.x, y2 = Last_class[i].detial[m][n].End_point.y;
                double c = pow(y2-y1,2) + pow(x1-x2,2);
                int h = Last_class[i].detial[m][n].Start_point.SET;//-35 > 0 ? Last_class[i].detial[m][n].Start_point.SET-35:0;
                int y = Last_class[i].detial[m][n].End_point.SET;//+35 > (int)laserCloudScansOrder[m].size() ? \
                                                                 //        (int)laserCloudScansOrder[m].size():Last_class[i].detial[m][n].End_point.SET+35;
                for(int k=h; k<y; k++)
                {
                    double a = (laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x-x1) * (y2-y1);
                    double b = (laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y-y1) * (x1-x2);
                    double dis = sqrt((a+b)*(a+b)/c);//点到直线的距离
                    if(dis>0.05)
                        continue;

                    std::vector<double> temp_point_mn;
                    temp_point_mn.push_back(pow(laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x,2) + \
                                                              pow(laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y,2));//直线kb上所有点到原点的距离
                    temp_point_mn.push_back(laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x);
                    temp_point_mn.push_back(laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y);
                    temp_point_mn.push_back(m);
                    temp_point_mn.push_back(n);
                    Point_set.push_back(temp_point_mn);
                } 
            }
        }
        if(Point_set.size()<2)
            continue;
        //默认第一列排序、 由小到大, 找到端点（其实不用排序，只要最大的，优化）
        std::sort(Point_set.begin(),Point_set.end());
        //距离最远的一定是直线kb的端点
        double Start_x = Point_set[Point_set.size()-1][1];
        double Start_y = Point_set[Point_set.size()-1][2];
        for(int m=0; m<Point_set.size(); m++)//kb上的点到端点的距离
        {
            double dis = pow(Point_set[m][1] - Start_x,2) + pow(Point_set[m][2] - Start_y,2);
            Point_set[m][0] = sqrt(dis);
        } 
        //默认第一列排序、 由小到大, 排序（假设kb确定是直线,理论上角度更合适，不过复杂点）
        std::sort(Point_set.begin(),Point_set.end());
        for(int m=0; m<Point_set.size()-1; m++)
        {
            double dis_euclidean = fabs(Point_set[m][0] - Point_set[m+1][0]);//表示连续点之间的距离
            int dis_lines = (int)fabs(Point_set[m][2] - Point_set[m+1][2]);//表示点所在的扫描线之间的距离   
            //扫描线间距必须大于等于1     
            if(dis_euclidean<0.25 || dis_lines<2)
                continue;
            //扫描线中缺省直线段位置
            //首尾点距离
            int tmp1 = (int)Point_set[m][3];//m
            int tmp2 = (int)Point_set[m][4];//n
            int tmp3 = (int)Point_set[m+1][3];//m+1
            int tmp4 = (int)Point_set[m+1][4];//n+1
            double tempStart_dis = pow(Last_class[i].detial[tmp1][tmp2].Start_point.x - Last_class[i].detial[tmp3][tmp4].End_point.x,2) + \
                                       pow(Last_class[i].detial[tmp1][tmp2].Start_point.y - Last_class[i].detial[tmp3][tmp4].End_point.y,2);
            //尾首点距离
            double tempEnd_dis = pow(Last_class[i].detial[tmp1][tmp2].End_point.x - Last_class[i].detial[tmp3][tmp4].Start_point.x,2) +   \
                                       pow(Last_class[i].detial[tmp1][tmp2].End_point.y - Last_class[i].detial[tmp3][tmp4].Start_point.y,2);

            //距离较小的两个点作为kb
            double x1, y1, x2, y2;//x2尾点 -> x1首点
            int h,y;
            if(tempStart_dis > tempEnd_dis) 
            {
                x1 = Last_class[i].detial[tmp3][tmp4].Start_point.x, y1 = Last_class[i].detial[tmp3][tmp4].Start_point.y;
                x2 = Last_class[i].detial[tmp1][tmp2].End_point.x, y2 = Last_class[i].detial[tmp1][tmp2].End_point.y;
                h = Last_class[i].detial[tmp1][tmp2].End_point.SET;
                y = Last_class[i].detial[tmp3][tmp4].Start_point.SET;
            }
            else
            {
                x1 = Last_class[i].detial[tmp1][tmp2].Start_point.x, y1 = Last_class[i].detial[tmp1][tmp2].Start_point.y;
                x2 = Last_class[i].detial[tmp3][tmp4].End_point.x, y2 = Last_class[i].detial[tmp3][tmp4].End_point.y;
                h = Last_class[i].detial[tmp3][tmp4].End_point.SET;
                y = Last_class[i].detial[tmp1][tmp2].Start_point.SET;
            }
                   
            double c = pow(y2-y1,2) + pow(x1-x2,2);
            //printf("OK5!!!!!!!!!\n");/////////////////////////////////////////////////////////
            //j扫面线的id 从小到大
            for(int j=min(tmp1,tmp3)+1; j<max(tmp1,tmp3); j++)
            {
                vector <vector<double> > temp_points;
                //printf("scanid: x:  %d   \n",j);/////////////////////////////////////////////////////////
                int temp_h = h-35 > 0 ? h-35:0;
                int temp_y = y+35 > (int)laserCloudScansOrder[j].size() ? (int)laserCloudScansOrder[j].size():y+35;
                //点到直线的距离
                if(temp_h < temp_y)//外线高于内线，近似平行，出现负角度
                {
                    //printf("left: start-end2: %d  %d   \n",temp_h,temp_y);
                    for(int k=temp_h; k<temp_y; k++)//右侧
                    {   
                        //printf("left:   %d   \n",k);
                        double a = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x-x1) * (y2-y1);
                        double b = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y-y1) * (x1-x2);
                        double dis = sqrt((a+b)*(a+b)/c);
                        if(dis>0.02)
                            continue;
                        vector<double> pcl_point;
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x);
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y);
                        pcl_point.push_back(k);//点在扫面线上的 移动 id
                        temp_points.push_back(pcl_point);
                    }
                }
                if(temp_h >= temp_y)
                {
                    //printf("right:start-end2: %d  %d   \n",temp_h,temp_y);
                    for(int k=temp_h; k<laserCloudScansOrder[j].size(); k++)//右侧
                    {
                        double a = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x-x1) * (y2-y1);
                        double b = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y-y1) * (x1-x2);
                        double dis = sqrt((a+b)*(a+b)/c);
                        if(dis>0.02)
                            continue;
                        vector<double> pcl_point;
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x);
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y);
                        pcl_point.push_back(k);//点在扫面线上的 移动 id
                        temp_points.push_back(pcl_point);
                    }
                    //printf("ok0 \n");
                    for(int k=0; k<temp_y; k++)
                    {
                        double a = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x-x1) * (y2-y1);
                        double b = (laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y-y1) * (x1-x2);
                        double dis = sqrt((a+b)*(a+b)/c);
                        if(dis>0.02)
                            continue;
                        vector<double> pcl_point;
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].x);
                        pcl_point.push_back(laserCloudScansOrder[j][laserCloudScansOrder[j].size()-k-1].y);
                        pcl_point.push_back(k);//点在扫面线上的 移动 id
                        temp_points.push_back(pcl_point);
                    }
                }
                //srvprintf("ok1 \n");
                if(temp_points.size()<10) 
                    continue;
                vector<vector<int> >index_temp_points;
                vector<int> temp_indes;
                temp_indes.push_back(0);
                for(int k=0; k<temp_points.size()-1; k++)
                {
                    temp_indes.push_back(k);
                    if(4 < fabs(temp_points[k][2] - temp_points[k+1][2]) || k >= temp_points.size()-2)//点间距大 （换成参数）
                    {
                        temp_indes.erase(temp_indes.begin());//删除第一个元素
                        index_temp_points.push_back(temp_indes);//存储有效id
                        temp_indes.push_back(0);//随意添加一个元素
                        temp_indes.erase(temp_indes.begin()+1, temp_indes.end());//只留下一个
                    }
                }
                //for(int k=0; k<index_temp_points.size(); k++)
                //{
                //     printf("index_temp_points size %d\n",(int)index_temp_points[k].size());
                //}
                //printf("OK4!!!!!!!!!\n");
                //分析这一扫描线上的 缺省线段的kb
                //斜率\截距\相关系数
                //函数：x = Ky + B  其中 K = d1x/d1y; B = x1-Ky1;  当x=0时的 截距 b = -b/k = y1 - x1/K
                double temp_K = std::atan2(x1-x2, y1-y2) * (180/M_PI);
                if(temp_K<0.0) temp_K = temp_K + 360;
                double temp_B;
                if(fabs(temp_K-90)<1e-6 || fabs(temp_K-270)<1e-6)  
                {
                    temp_B = y1;//垂直
                }
                else if(temp_K<1e-6 || 360-temp_K<1e-6 || fabs(temp_K-180)<1e-6)  
                {
                    temp_B = 36.0;//水平
                }
                else   
                {
                    temp_B = y1 - x1/std::tan(temp_K*M_PI/180);//输入的是弧度 
                    if(temp_B>36) temp_B = 36;
                    if(temp_B<-36) temp_B = -36;
                }
                //printf("LineFitLeastSquares  temp_K: %f   temp_B: %f %f %f %f %f \n",temp_K,temp_B,x1,y1,x2,y2);/////////////////////////////////////////////////////////
                temp_K = std::atan((x1-x2)/(y1-y2)) * (180/M_PI);
                //printf("LineFitLeastSquares  temp_K: %f   \n",temp_K);

                //把近点  最小二乘拟合，根据kbr选择有效的缺省直线段
                for(int k=0; k<index_temp_points.size(); k++)
                {
                    if(10 > index_temp_points[k].size())//点的数目不够（换成参数）
                        continue;
                    double temp_dis = pow(temp_points[index_temp_points[k][0]][0] - temp_points[index_temp_points[k][index_temp_points[k].size()-1]][0],2) + \
                                      pow(temp_points[index_temp_points[k][0]][1] - temp_points[index_temp_points[k][index_temp_points[k].size()-1]][1],2);
                    if(0.25 > sqrt(temp_dis))//线段长度不够（换成参数）
                        continue;

                    vector<double> return_KB;
                    LineFitLeastSquares(index_temp_points[k], temp_points, return_KB);
                    if(fabs(return_KB[0]-temp_K)>thrd_K+1 || fabs(return_KB[1]-temp_B)>thrd_B+0.05)
                        continue;

                    Kmeans_line temp_point;//RESIZE VECTOR
                    Last_class[i].Lines_num++;
                    temp_point.Id_16lines = j;					 //该条线的线的ID
                    //temp_point.Id_alllines = 0;				 //该条线总数目ID
                    temp_point.K_k = return_KB[0];
                    temp_point.K_b = return_KB[1];
                    temp_point.Start_point.x =temp_points[*index_temp_points[k].begin()][0];//该线段首尾点的信息
                    temp_point.Start_point.y =temp_points[*index_temp_points[k].begin()][1];//该线段首尾点的信息
                    temp_point.Start_point.SET =temp_points[*index_temp_points[k].begin()][2];//该线段首尾点的信息
                    temp_point.End_point.x =temp_points[*(index_temp_points[k].end()-1)][0];//该线段首尾点的信息
                    temp_point.End_point.y =temp_points[*(index_temp_points[k].end()-1)][1];//该线段首尾点的信息
                    temp_point.End_point.SET =temp_points[*(index_temp_points[k].end()-1)][2];//该线段首尾点的信息
                    Last_class[i].detial[j].push_back(temp_point);
                }
            }
        }
    }
    //printf("bbbbbbbbbbbbbbbbb\n");///////////////////////////////////////////////////////////	
    //向两侧延伸
    for(int i=0; i<K_means; i++)   //所有端点的距离最长的端点（首尾端点就好）
    {
        if(Last_class[i].Lines_num<2)
            continue;
        //printf("OKS\n");/////////////////////////////////////////////////////////
        //找到障碍物角度范围，计算斜率和截距
        std::vector<Point_mn> points_ang_start;
        std::vector<Point_mn> points_ang_end;
        Point_mn temp_point;
        vector<int> ScanID;
        for(int m=0; m<Ground_line; m++)
        {
            if(Last_class[i].detial[m].size()>0)
                ScanID.push_back(m);
            else continue;
            for(int n=0; n<Last_class[i].detial[m].size(); n++)
            {
                temp_point.x = Last_class[i].detial[m][n].Start_point.x;
                temp_point.y = Last_class[i].detial[m][n].Start_point.y;
                temp_point.m = m;
                temp_point.n = n;
                points_ang_start.push_back(temp_point);
                temp_point.x = Last_class[i].detial[m][n].End_point.x;
                temp_point.y = Last_class[i].detial[m][n].End_point.y;
                temp_point.m = m;
                temp_point.n = n;
                points_ang_end.push_back(temp_point);
            }
        }
        if((int)points_ang_start.size()+(int)points_ang_end.size() <1)
            continue;
        //printf("OKS1\n");/////////////////////////////////////////////////////////
        //存储端点
        int Start_start=0, Start_end=0;
        int End_start=0, End_end=0;
        double thold_dis = 0;
        for(int m=0; m<points_ang_start.size(); m++)//首点
        {
            for(int n=0; n<points_ang_end.size(); n++)//对应  尾点
            {
                //printf("x_y:    %f  %f    %f  %f\n",points_ang_sor[m].x,points_ang_sor[m].y, points_ang_sor[n].x,points_ang_sor[n].y);  
                double temp_x = (points_ang_start[m].x -  points_ang_end[n].x)*(points_ang_start[m].x -  points_ang_end[n].x);
                double temp_y = (points_ang_start[m].y -  points_ang_end[n].y)*(points_ang_start[m].y -  points_ang_end[n].y);
                double temp_dis = sqrt(temp_x + temp_y);
                if(temp_dis < thold_dis)
                    continue;
                thold_dis = temp_dis;
                Start_start = points_ang_start[m].m;//扫面线id
                Start_end = points_ang_start[m].n;//第几个直线段
                End_start = points_ang_end[n].m;
                End_end = points_ang_end[n].n;
            }
        }
        if(ScanID.size()<2 && fabs(Start_start-End_start)>2)//跳跃
            continue;
        /////////////////////////////////////////////////////////
        //printf("OKS2\n");
        //printf("id : %d %d %d  %d\n",Start_start,Start_end,End_start,End_end);
        //printf("聚类的端点： Start  %f  %f   %f  %f\n",Last_class[i].detial[Start_start][Start_end].Start_point.x,Last_class[i].detial[Start_start][Start_end].Start_point.y,\
                                                      Last_class[i].detial[End_start][End_end].End_point.x,Last_class[i].detial[End_start][End_end].End_point.y);
        //printf("OKS3\n");
        /////////////////////////////////////////////////////////
        std::sort(ScanID.begin(),ScanID.end());
        double Start_ang = std::atan2(Last_class[i].detial[Start_start][Start_end].Start_point.x, Last_class[i].detial[Start_start][Start_end].Start_point.y) * (180/M_PI);
        double End_ang = std::atan2(Last_class[i].detial[End_start][End_end].End_point.x, Last_class[i].detial[End_start][End_end].End_point.y) * (180/M_PI);

        //起始处
        if(Start_start != 7 && Start_start >= ScanID[0] && ScanID.size()>1)  //????
        {    
            //printf("OK11!!!!!!!!!\n");/////////////////////////////////////////////////////////
            double x1 = Last_class[i].detial[Start_start][Start_end].Start_point.x, y1 = Last_class[i].detial[Start_start][Start_end].Start_point.y;
            double x2 = Last_class[i].detial[Start_start][Start_end].End_point.x, y2 = Last_class[i].detial[Start_start][Start_end].End_point.y;  
            double c = pow(y2-y1,2) + pow(x1-x2,2);
            if(sqrt(c) > 10.0)  
                continue; 
            vector <vector<double> > temp_points;
            int h = 0;
            for(int k=0; k<laserCloudScansOrder[Start_start + 1].size(); k++)
            {
                double temp = std::atan2(laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].x,\
                                                    laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].y) * (180/M_PI);

                if(fabs(temp - Start_ang)<2)//跨度20个点
                {
                    h = k;
                    break;
                }
            }
            int temp_h = h-55>0 ? h-55 : (int)laserCloudScansOrder[Start_start+1].size()+h-55;
            int temp_y = h+20>(int)laserCloudScansOrder[Start_start+1].size() ? h+20-(int)laserCloudScansOrder[Start_start+1].size() : h+20;
            //printf("聚类的端点1 %d  %d  %d  %f\n",h,temp_h,temp_y,Start_ang);
            if(temp_h<temp_y)
            {
                //printf("聚类的端点2 %d  %d \n",temp_h,temp_y);
                for(int k=temp_h; k<temp_y; k++)//对于每一条扫描线上的 线段
                {
                    double a = (laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].x-x1) * (y2-y1);
                    double b = (laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].y-y1) * (x1-x2);
                    double dis = sqrt((a+b)*(a+b)/c);//点到直线的距离
                    if(dis>0.03)
                        continue;
                    vector<double> pcl_point;
                    pcl_point.push_back(laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].x);
                    pcl_point.push_back(laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].y);
                    pcl_point.push_back(k);//点在扫面线上的 移动 id
                    temp_points.push_back(pcl_point);

                }
            }
            else
            {
                //printf("聚类的端点3 %d  %d  \n",temp_h,temp_y);
                for(int k=temp_h; k<laserCloudScansOrder[Start_start + 1].size(); k++)//右侧
                {
                    double a = (laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].x-x1) * (y2-y1);
                    double b = (laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].y-y1) * (x1-x2);
                    double dis = sqrt((a+b)*(a+b)/c);
                    if(dis>0.03)
                        continue;
                    vector<double> pcl_point;
                    pcl_point.push_back(laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].x);
                    pcl_point.push_back(laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].y);
                    pcl_point.push_back(k);//点在扫面线上的 移动 id
                    temp_points.push_back(pcl_point);

                }
                for(int k=0; k<temp_y; k++)
                {
                    double a = (laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].x-x1) * (y2-y1);
                    double b = (laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].y-y1) * (x1-x2);
                    double dis = sqrt((a+b)*(a+b)/c);
                    if(dis>0.03)
                        continue;
                    vector<double> pcl_point;
                    pcl_point.push_back(laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].x);
                    pcl_point.push_back(laserCloudScansOrder[Start_start + 1][laserCloudScansOrder[Start_start + 1].size()-k-1].y);
                    pcl_point.push_back(k);//点在扫面线上的 移动 id
                    temp_points.push_back(pcl_point);

                }
            }
            if(temp_points.size()<8) 
                continue;
            vector<vector<int> >index_temp_points;
            vector<int> temp_indes;
            temp_indes.push_back(0);
            for(int k=0; k<temp_points.size()-1; k++)
            {
                temp_indes.push_back(k);
                if(4 < fabs(temp_points[k][2] - temp_points[k+1][2]) || k >= temp_points.size()-2)//点间距大 （换成参数）
                {
                    temp_indes.erase(temp_indes.begin());//删除第一个元素
                    index_temp_points.push_back(temp_indes);//存储有效id
                    temp_indes.push_back(0);//随意添加一个元素
                    temp_indes.erase(temp_indes.begin()+1, temp_indes.end());//只留下一个
                }
            }
            //printf("OK12!!!!!!!!!\n");/////////////////////////////////////////////////////////
            //函数：x = Ky + B  其中 K = d1x/d1y; B = x1-Ky1;  当x=0时的 截距 b = -b/k = y1 - x1/K
            double temp_K = std::atan2(x1-x2, y1-y2) * (180/M_PI);
            if(temp_K<0.0) temp_K = temp_K + 360;
            double temp_B;
            if(fabs(temp_K-90)<1e-6 || fabs(temp_K-270)<1e-6)  
            {
                temp_B = y1;//垂直
            }
            else if(temp_K<1e-6 || 360-temp_K<1e-6 || fabs(temp_K-180)<1e-6)  
            {
                temp_B = 36.0;//水平
            }
            else   
            {
                temp_B = y1 - x1/std::tan(temp_K*M_PI/180);//输入的是弧度 
                if(temp_B>36) temp_B = 36;
                if(temp_B<-36) temp_B = -36;
            }
            //printf("OK0!!!!!!!!!\n");
            //printf("LineFitLeastSquares  temp_K: %f   temp_B: %f %f %f %f %f \n",temp_K,temp_B,x1,y1,x2,y2);/////////////////////////////////////////////////////////
            temp_K = std::atan((x1-x2)/(y1-y2)) * (180/M_PI);
            //printf("LineFitLeastSquares  temp_K: %f   \n",temp_K);
            //printf("OK13!!!!!!!!!\n");/////////////////////////////////////////////////////////
            //把近点  最小二乘拟合，根据kbr选择有效的缺省直线段
            for(int k=0; k<index_temp_points.size(); k++)
            {
                //printf("OK14!!!!!!!!!\n");/////////////////////////////////////////////////////////
                if(8 > index_temp_points[k].size())//点的数目不够（换成参数）
                    continue;
                double temp_dis = pow(temp_points[index_temp_points[k][0]][0] - temp_points[index_temp_points[k][index_temp_points[k].size()-1]][0],2) + \
                                    pow(temp_points[index_temp_points[k][0]][1] - temp_points[index_temp_points[k][index_temp_points[k].size()-1]][1],2);
                if(0.25 > sqrt(temp_dis))//线段长度不够（换成参数）
                    continue;

                vector<double> return_KB;
                LineFitLeastSquares(index_temp_points[k], temp_points, return_KB);
                if(fabs(return_KB[0]-temp_K)>thrd_K+1 || fabs(return_KB[1]-temp_B)>thrd_B+0.05)
                    continue;
                //printf("OK15!!!!!!!!!\n");b
                Kmeans_line temp_point;//RESIZE VECTOR
                Last_class[i].Lines_num++;
                temp_point.Id_16lines = Start_start+1;					 //该条线的线的ID
                //temp_point.Id_alllines = 0;				 //该条线总数目ID
                temp_point.K_k = return_KB[0];
                temp_point.K_b = return_KB[1];
                temp_point.Start_point.x =temp_points[*index_temp_points[k].begin()][0];//该线段首尾点的信息
                temp_point.Start_point.y =temp_points[*index_temp_points[k].begin()][1];//该线段首尾点的信息
                temp_point.Start_point.SET =temp_points[*index_temp_points[k].begin()][2];//该线段首尾点的信息
                temp_point.End_point.x =temp_points[*(index_temp_points[k].end()-1)][0];//该线段首尾点的信息
                temp_point.End_point.y =temp_points[*(index_temp_points[k].end()-1)][1];//该线段首尾点的信息/！！！！！！！！！！！！！！！！！应该用kb
                temp_point.End_point.SET =temp_points[*(index_temp_points[k].end()-1)][2];//该线段首尾点的信息
                Last_class[i].detial[Start_start + 1].push_back(temp_point);
                //printf("OK16!!!!!!!!!\n");/////////////////////////////////////////////////////////
                /*for(int p=0; p<index_temp_points[k].size(); p++)
                {
                    pcl::PointXYZI point;
                    point.x = temp_points[index_temp_points[k][p]][0];
                    point.y = temp_points[index_temp_points[k][p]][1];
                    point.z = 0;
                    Line_ptr->push_back(point);
                }*/

            }
            //printf("OK17!!!!!!!!!\n");/////////////////////////////////////////////////////////
        }
            //printf("OK!!!!!!!!!\n");///////////////////////////////////////////////////////////	
        //末尾处
        if(End_start != 7 && End_start >= ScanID[0] && ScanID.size()>1)
        {    
            //printf("OK00!!!!!!!!!\n");/////////////////////////////////////////////////////////
            double x1 = Last_class[i].detial[End_start][End_end].Start_point.x, y1 = Last_class[i].detial[End_start][End_end].Start_point.y;
            double x2 = Last_class[i].detial[End_start][End_end].End_point.x, y2 = Last_class[i].detial[End_start][End_end].End_point.y;  
            double c = pow(y2-y1,2) + pow(x1-x2,2);
            if(sqrt(c) > 10.0)  
                continue; 
            vector <vector<double> > temp_points;
            int h = 0;
            for(int k=0; k<laserCloudScansOrder[End_start + 1].size(); k++)
            {
                double temp = std::atan2(laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].x,\
                                                    laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].y) * (180/M_PI);
                if(fabs(temp - End_ang)<2)//跨度20个点
                {
                    h = k;
                    break;
                }
            }
            int temp_h = h-20>0 ? h-20 : (int)laserCloudScansOrder[End_start+1].size()+h-22;
            int temp_y = h+55>(int)laserCloudScansOrder[End_start+1].size() ? h+55-(int)laserCloudScansOrder[End_start+1].size() : h+55;
            //printf("聚类的端点1 %d  %d  %d  %f\n",h,temp_h,temp_y,Start_ang);/////////////////////////////////////////////////////////
            
            if(temp_h<temp_y)
            {
                //printf("聚类的端点2 %d  %d\n",temp_h,temp_y);/////////////////////////////////////////////////////////
                for(int k=temp_h; k<temp_y; k++)//对于每一条扫描线上的 线段
                {
                    double a = (laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].x-x1) * (y2-y1);
                    double b = (laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].y-y1) * (x1-x2);
                    double dis = sqrt((a+b)*(a+b)/c);//点到直线的距离
                    if(dis>0.03)
                        continue;
                    vector<double> pcl_point;
                    pcl_point.push_back(laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].x);
                    pcl_point.push_back(laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].y);
                    pcl_point.push_back(k);//点在扫面线上的 移动 id
                    temp_points.push_back(pcl_point);

                }
            }
            else
            {
                //printf("聚类的端点2 %d  %d\n",temp_h,temp_y);/////////////////////////////////////////////////////////
                for(int k=temp_h; k<laserCloudScansOrder[End_start + 1].size(); k++)//右侧
                {
                    double a = (laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].x-x1) * (y2-y1);
                    double b = (laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].y-y1) * (x1-x2);
                    double dis = sqrt((a+b)*(a+b)/c);
                    if(dis>0.03)
                        continue;
                    vector<double> pcl_point;
                    pcl_point.push_back(laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].x);
                    pcl_point.push_back(laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].y);
                    pcl_point.push_back(k);//点在扫面线上的 移动 id
                    temp_points.push_back(pcl_point);

                }
                for(int k=0; k<temp_y; k++)
                {
                    double a = (laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].x-x1) * (y2-y1);
                    double b = (laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].y-y1) * (x1-x2);
                    double dis = sqrt((a+b)*(a+b)/c);
                    if(dis>0.03)
                        continue;
                    vector<double> pcl_point;
                    pcl_point.push_back(laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].x);
                    pcl_point.push_back(laserCloudScansOrder[End_start + 1][laserCloudScansOrder[End_start + 1].size()-k-1].y);
                    pcl_point.push_back(k);//点在扫面线上的 移动 id
                    temp_points.push_back(pcl_point);

                }
            }
            if(temp_points.size()<8) 
                continue;
            vector<vector<int> >index_temp_points;
            vector<int> temp_indes;
            temp_indes.push_back(0);
            for(int k=0; k<temp_points.size()-1; k++)
            {
                temp_indes.push_back(k);
                if(4 < fabs(temp_points[k][2] - temp_points[k+1][2]) || k >= temp_points.size()-2)//点间距大 （换成参数）
                {
                    temp_indes.erase(temp_indes.begin());//删除第一个元素
                    index_temp_points.push_back(temp_indes);//存储有效id
                    temp_indes.push_back(0);//随意添加一个元素
                    temp_indes.erase(temp_indes.begin()+1, temp_indes.end());//只留下一个
                }
            }

            //函数：x = Ky + B  其中 K = d1x/d1y; B = x1-Ky1;  当x=0时的 截距 b = -b/k = y1 - x1/K
            double temp_K = std::atan2(x1-x2, y1-y2) * (180/M_PI);
            if(temp_K<0.0) temp_K = temp_K + 360;
            double temp_B;
            if(fabs(temp_K-90)<1e-6 || fabs(temp_K-270)<1e-6)  
            {
                temp_B = y1;//垂直
            }
            else if(temp_K<1e-6 || 360-temp_K<1e-6 || fabs(temp_K-180)<1e-6)  
            {
                temp_B = 36.0;//水平
            }
            else   
            {
                temp_B = y1 - x1/std::tan(temp_K*M_PI/180);//输入的是弧度 
                if(temp_B>36) temp_B = 36;
                if(temp_B<-36) temp_B = -36;
            }
            //printf("OK!!!!!!!!!\n");/////////////////////////////////////////////////////////
            //printf("LineFitLeastSquares  temp_K: %f   temp_B: %f %f %f %f %f \n",temp_K,temp_B,x1,y1,x2,y2);/////////////////////////////////////////////////////////
            temp_K = std::atan((x1-x2)/(y1-y2)) * (180/M_PI);
            //printf("LineFitLeastSquares  temp_K: %f   \n",temp_K);

            //把近点  最小二乘拟合，根据kbr选择有效的缺省直线段
            for(int k=0; k<index_temp_points.size(); k++)
            {
                //printf("OK1!!!!!!!!!\n");/////////////////////////////////////////////////////////
                if(8 > index_temp_points[k].size())//点的数目不够（换成参数）
                    continue;
                //printf("OK2!!!!!!!!!\n");  /////////////////////////////////////////////////////////
                double temp_dis = pow(temp_points[index_temp_points[k][0]][0] - temp_points[index_temp_points[k][index_temp_points[k].size()-1]][0],2) + \
                                    pow(temp_points[index_temp_points[k][0]][1] - temp_points[index_temp_points[k][index_temp_points[k].size()-1]][1],2);
                //printf("OK3!!!!!!!!!\n");/////////////////////////////////////////////////////////
                if(0.25 > sqrt(temp_dis))//线段长度不够（换成参数）
                    continue;
                //printf("OK4!!!!!!!!!\n");/////////////////////////////////////////////////////////
                vector<double> return_KB;
                LineFitLeastSquares(index_temp_points[k], temp_points, return_KB);
                if(fabs(return_KB[0]-temp_K)>thrd_K+1 || fabs(return_KB[1]-temp_B)>thrd_B+0.05)
                    continue;
                //printf("OK5!!!!!!!!!\n");/////////////////////////////////////////////////////////
                Kmeans_line temp_point;//RESIZE VECTOR
                Last_class[i].Lines_num++;
                temp_point.Id_16lines = End_start+1;					 //该条线的线的ID
                //temp_point.Id_alllines = 0;				 //该条线总数目ID
                temp_point.K_k = return_KB[0];
                temp_point.K_b = return_KB[1];
                temp_point.Start_point.x =temp_points[*index_temp_points[k].begin()][0];//该线段首尾点的信息
                temp_point.Start_point.y =temp_points[*index_temp_points[k].begin()][1];//该线段首尾点的信息
                temp_point.Start_point.SET =temp_points[*index_temp_points[k].begin()][2];//该线段首尾点的信息
                temp_point.End_point.x =temp_points[*(index_temp_points[k].end()-1)][0];//该线段首尾点的信息
                temp_point.End_point.y =temp_points[*(index_temp_points[k].end()-1)][1];//该线段首尾点的信息/！！！！！！！！！！！！！！！！！应该用kb
                temp_point.End_point.SET =temp_points[*(index_temp_points[k].end()-1)][2];//该线段首尾点的信息
                Last_class[i].detial[End_start + 1].push_back(temp_point);
            //printf("OK6!!!!!!!!!\n");/////////////////////////////////////////////////////////
            }
            //printf("OK7!!!!!!!!!\n");/////////////////////////////////////////////////////////
        }
   //printf("OK8!!!!!!!!!\n");/////////////////////////////////////////////////////////
    }
    //printf("cccccccccccc\n");/////////////////////////////////////////////////////////
/*
     //去掉重复的直线段
    for(int i=0; i<K_means ; i++)  //类中
    {
        if(Last_class[i].Lines_num<2)//0/1
            continue;
        //检测每一条线
        for(int m=0; m<Ground_line; m++)//对于每一条扫描线
        {
            if(Last_class[i].detial[m].size() < 2 )
                continue;
            for(int j=0; j<Last_class[i].detial[m].size(); j++)
            {
                for(int k=j+1; k<Last_class[i].detial[m].size(); k++)
                {

                    int a1 = Last_class[i].detial[m][j].Start_point.SET;
                    int b1 = Last_class[i].detial[m][j].End_point.SET;
                    int a2 = Last_class[i].detial[m][k].Start_point.SET;
                    int b2 = Last_class[i].detial[m][k].End_point.SET;
                    if(a1<=a2 && a2<=b1  && a1<=b2 && b2<=b1)   //双向，只考虑j包含k   a1  a2  b2  b1
                    {
                        Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+k);
                        Last_class[i].Lines_num--;
                        k--;
                        printf("去掉重复的直线段1\n");
                    }
                    else if(a2<=a1 && a2<=b1  && a1<=b2 && b1<=b2)   //双向，只考虑j包含k   a2  a1  b1  b2
                    {
                        Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+j);
                        Last_class[i].Lines_num--;
                        j--;
                        printf("去掉重复的直线段2\n");
                    }
                    else if(a1<=a2 && a2<=b1  && a2<=b1 && b1<=b2)   //双向，只考虑j包含k   a1  a2  b1  b2
                    {
                        Kmeans_line temp_point;//RESIZE VECTOR
                        temp_point.Id_16lines = m;					 //该条线的线的ID
                        //temp_point.Id_alllines = 0;				 //该条线总数目ID
                        temp_point.K_k = (Last_class[i].detial[m][j].K_k + Last_class[i].detial[m][k].K_k)/2.0;
                        temp_point.K_b = (Last_class[i].detial[m][j].K_b + Last_class[i].detial[m][k].K_b)/2.0;
                        temp_point.Start_point.x = Last_class[i].detial[m][j].Start_point.x;
                        temp_point.Start_point.y = Last_class[i].detial[m][j].Start_point.y;
                        temp_point.Start_point.SET = Last_class[i].detial[m][j].Start_point.SET;

                        temp_point.End_point.x = Last_class[i].detial[m][k].Start_point.x;
                        temp_point.End_point.y = Last_class[i].detial[m][k].Start_point.y;
                        temp_point.End_point.SET =Last_class[i].detial[m][k].Start_point.SET;

                        Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+k);
                        Last_class[i].Lines_num--;
                        Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+j);
                        Last_class[i].Lines_num--;
                        k = k-2;
                        j = j-1;

                        Last_class[i].detial[m].push_back(temp_point);
                        printf("去掉重复的直线段3\n");
                    }
                    else if(a2<=a1 && a1<=b2  && a1<=b2 && b2<=b1)   //双向，只考虑j包含k   a2  a1  b2  b1
                    {
                        Kmeans_line temp_point;//RESIZE VECTOR
                        temp_point.Id_16lines = m;					 //该条线的线的ID
                        //temp_point.Id_alllines = 0;				 //该条线总数目ID
                        temp_point.K_k = (Last_class[i].detial[m][j].K_k + Last_class[i].detial[m][k].K_k)/2.0;
                        temp_point.K_b = (Last_class[i].detial[m][j].K_b + Last_class[i].detial[m][k].K_b)/2.0;
                        temp_point.Start_point.x = Last_class[i].detial[m][k].Start_point.x;
                        temp_point.Start_point.y = Last_class[i].detial[m][k].Start_point.y;
                        temp_point.Start_point.SET = Last_class[i].detial[m][k].Start_point.SET;

                        temp_point.End_point.x = Last_class[i].detial[m][j].Start_point.x;
                        temp_point.End_point.y = Last_class[i].detial[m][j].Start_point.y;
                        temp_point.End_point.SET =Last_class[i].detial[m][j].Start_point.SET;

                        Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+k);//k大，多减去一个
                        Last_class[i].Lines_num--;
                        Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+j);
                        Last_class[i].Lines_num--;
                        k = k-2;
                        j = j-1;

                        Last_class[i].detial[m].push_back(temp_point);
                        printf("去掉重复的直线段4\n");
                    }
                }
            }
        }
    }*/
    /*
     //去掉重复的直线段
     for(int i=0; i<K_means ; i++)  //类中
    {
        for(int m=0; m<Ground_line; m++)
        {
            for(int j=0; j<Last_class[i].detial[m].size(); j++)//1
            {
                int a1 = Last_class[i].detial[m][j].Start_point.SET;
                int b1 = Last_class[i].detial[m][j].End_point.SET;
                for(int ii=i+1; ii<K_means ; ii++)  //类中
                {
                    for(int jj=0; jj<Last_class[i].detial[m].size(); jj++)
                    {
                        int a2 = Last_class[ii].detial[m][jj].Start_point.SET;
                        int b2 = Last_class[ii].detial[m][jj].End_point.SET;

                        if(a1<=a2 && a2<=b1  && a1<=b2 && b2<=b1)   //双向，只考虑j包含k   a1  a2  b2  b1
                        {
                            Last_class[ii].detial[m].erase(Last_class[ii].detial[m].begin()+jj);
                            Last_class[ii].Lines_num--;
                            jj--;
                            printf("去掉重复的直线段1\n");
                        }
                        else if(a2<=a1 && a2<=b1  && a1<=b2 && b1<=b2)   //双向，只考虑j包含k   a2  a1  b1  b2
                        {
                            Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+j);
                            Last_class[i].Lines_num--;
                            j--;
                            printf("去掉重复的直线段2\n");
                        }
                        else if(a1<=a2 && a2<=b1  && a2<=b1 && b1<=b2)   //双向，只考虑j包含k   a1  a2  b1  b2
                        {
                            Kmeans_line temp_point;//RESIZE VECTOR
                            temp_point.Id_16lines = m;					 //该条线的线的ID
                            //temp_point.Id_alllines = 0;				 //该条线总数目ID
                            temp_point.K_k = (Last_class[i].detial[m][j].K_k + Last_class[i].detial[m][k].K_k)/2.0;
                            temp_point.K_b = (Last_class[i].detial[m][j].K_b + Last_class[i].detial[m][k].K_b)/2.0;
                            temp_point.Start_point.x = Last_class[i].detial[m][j].Start_point.x;
                            temp_point.Start_point.y = Last_class[i].detial[m][j].Start_point.y;
                            temp_point.Start_point.SET = Last_class[i].detial[m][j].Start_point.SET;

                            temp_point.End_point.x = Last_class[i].detial[m][k].Start_point.x;
                            temp_point.End_point.y = Last_class[i].detial[m][k].Start_point.y;
                            temp_point.End_point.SET =Last_class[i].detial[m][k].Start_point.SET;

                            Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+k);
                            Last_class[i].Lines_num--;
                            Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+j);
                            Last_class[i].Lines_num--;
                            k = k-2;
                            j = j-1;

                            Last_class[i].detial[m].push_back(temp_point);
                            printf("去掉重复的直线段3\n");
                        }
                        else if(a2<=a1 && a1<=b2  && a1<=b2 && b2<=b1)   //双向，只考虑j包含k   a2  a1  b2  b1
                        {
                            Kmeans_line temp_point;//RESIZE VECTOR
                            temp_point.Id_16lines = m;					 //该条线的线的ID
                            //temp_point.Id_alllines = 0;				 //该条线总数目ID
                            temp_point.K_k = (Last_class[i].detial[m][j].K_k + Last_class[i].detial[m][k].K_k)/2.0;
                            temp_point.K_b = (Last_class[i].detial[m][j].K_b + Last_class[i].detial[m][k].K_b)/2.0;
                            temp_point.Start_point.x = Last_class[i].detial[m][k].Start_point.x;
                            temp_point.Start_point.y = Last_class[i].detial[m][k].Start_point.y;
                            temp_point.Start_point.SET = Last_class[i].detial[m][k].Start_point.SET;

                            temp_point.End_point.x = Last_class[i].detial[m][j].Start_point.x;
                            temp_point.End_point.y = Last_class[i].detial[m][j].Start_point.y;
                            temp_point.End_point.SET =Last_class[i].detial[m][j].Start_point.SET;

                            Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+k);//k大，多减去一个
                            Last_class[i].Lines_num--;
                            Last_class[i].detial[m].erase(Last_class[i].detial[m].begin()+j);
                            Last_class[i].Lines_num--;
                            k = k-2;
                            j = j-1;

                            Last_class[i].detial[m].push_back(temp_point);
                            printf("去掉重复的直线段4\n");

                }
            }
        }
    }*/



    // Create the vertices for the points and lines
    
    visualization_msgs::Marker line_list;
    line_list.header.frame_id = "/velodyne";
    line_list.header.stamp = ros::Time::now();
    line_list.ns = "lines";
    line_list.action = visualization_msgs::Marker::ADD;
    line_list.pose.orientation.w = 1.0;
    line_list.id = 2;
    line_list.type = visualization_msgs::Marker::LINE_LIST;
    // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
    line_list.scale.x = 0.2;
    line_list.scale.y = 0.2;
    line_list.scale.z = 0.2;
    line_list.color.a = 1.0;
    line_list.color.r = 1.0f;
    line_list.color.g = 1.0f;
    line_list.color.b = 1.0f;


     //显示
    for(int i=0; i<K_means ; i++)  
    {
        if(Last_class[i].Lines_num<2)//0/1 直接丢弃
            continue;
        vector<vector<double> > extreme_point;//端点
        for(int m=0; m<Ground_line; m++)//对于每一条扫描线
        {
            if(Last_class[i].detial[m].size() == Last_class[i].Lines_num )
                break;
            for(int n=0; n<Last_class[i].detial[m].size(); n++)//对于每一条扫描线上的 线段
            {
                vector<double> temp;
                double x1 = Last_class[i].detial[m][n].Start_point.x, y1 = Last_class[i].detial[m][n].Start_point.y;
                double x2 = Last_class[i].detial[m][n].End_point.x, y2 = Last_class[i].detial[m][n].End_point.y;
                temp.push_back(x1);
                temp.push_back(y1);
                temp.push_back(x2);
                temp.push_back(y2); 
                extreme_point.push_back(temp);
            }
        }
        if(extreme_point.size()>0)
        {
            for(int j=0; j<extreme_point.size(); j++)//size()是增加的
            {
                double x1 = extreme_point[j][0], y1 = extreme_point[j][1];
                double x2 = extreme_point[j][2], y2 = extreme_point[j][3];
                double temp_dis = sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
                if(temp_dis>1.0)
                {
                    vector<double> temp_1;
                    extreme_point.erase(extreme_point.begin()+j);//删除当前一行
                    temp_1.push_back(x1);
                    temp_1.push_back(y1);
                    temp_1.push_back((x2+x1)/2.0);
                    temp_1.push_back((y2+y1)/2.0); 
                    extreme_point.push_back(temp_1);
                    vector<double> temp_2;
                    temp_2.push_back((x2+x1)/2.0);
                    temp_2.push_back((y2+y1)/2.0);
                    temp_2.push_back(x2);
                    temp_2.push_back(y2); 
                    extreme_point.push_back(temp_2);
                    j--;
                }
            }

            //找端点
            vector<vector<double> >single_extreme_point;
            for(int j=0; j<extreme_point.size(); j++)//点到原点的距离
            {
                vector<double> temp;
                double x1 = extreme_point[j][0], y1 = extreme_point[j][1];
                double x2 = extreme_point[j][2], y2 = extreme_point[j][3];
                temp.push_back(x1*x1 + y1*y1);
                temp.push_back(x1);
                temp.push_back(y1);
                single_extreme_point.push_back(temp);
                vector<double> temp_1;
                temp_1.push_back(x2*x2 + y2*y2);
                temp_1.push_back(x2);
                temp_1.push_back(y2);
                single_extreme_point.push_back(temp_1);
            }
            sort(single_extreme_point.rbegin(),single_extreme_point.rend());//从大到小
            //点到端点的距离
            single_extreme_point[0][0] = 0;
            double start_X = single_extreme_point[0][1], start_Y = single_extreme_point[0][2];
            for(int j=1; j<single_extreme_point.size(); j++)
            {
                double x1 = single_extreme_point[j][1], y1 = single_extreme_point[j][2];
                single_extreme_point[j][0] = sqrt((x1-start_X)*(x1-start_X) + (y1-start_Y)*(y1-start_Y));
            }
            sort(single_extreme_point.begin(),single_extreme_point.end());//从小到大

            for(int j=0; j<single_extreme_point.size()-1; j++)//端点
            {
                if(j==0)//起始点
                {
                    Point temp;
                    temp.x = single_extreme_point[j][1];
                    temp.y = single_extreme_point[j][2];
                    ext_points.push_back(temp);  
                    //cout<<"point: "<<"    "<<single_extreme_point[j][1]<<"    "<<single_extreme_point[j][2]<<endl;          
                }
                if(j==single_extreme_point.size()-2)//终止点
                {
                    Point temp;
                    temp.x = single_extreme_point[j+1][1];
                    temp.y = single_extreme_point[j+1][2];
                    ext_points.push_back(temp);   
                    //cout<<"point: "<<"    "<<single_extreme_point[j+1][1]<<"    "<<single_extreme_point[j+1][2]<<endl;                             
                }
                if(fabs(single_extreme_point[j][0]-single_extreme_point[j+1][0])<LINETOLINE_DIS)
                {
                    geometry_msgs::Point p;
                    p.x = single_extreme_point[j][1];
                    p.y = single_extreme_point[j][2];
                    p.z = 0.1;
                    line_list.points.push_back(p);

                    p.x = single_extreme_point[j+1][1];
                    p.y = single_extreme_point[j+1][2];
                    p.z = 0.1;
                    line_list.points.push_back(p);
                }
                else
                {
                    Point temp;
                    temp.x = single_extreme_point[j][1];
                    temp.y = single_extreme_point[j][2];
                    ext_points.push_back(temp);    
                    //cout<<"point: "<<"    "<<single_extreme_point[j][1]<<"    "<<single_extreme_point[j][2]<<endl;          
                    temp.x = single_extreme_point[j+1][1];
                    temp.y = single_extreme_point[j+1][2];
                    ext_points.push_back(temp);   
                    //cout<<"point: "<<"    "<<single_extreme_point[j+1][1]<<"    "<<single_extreme_point[j+1][2]<<endl;           
                }
            }

        } 


        pcl::PointXYZI point;
        //检测每一条线
        std::vector<std::vector<double> > Point_set;
        for(int m=0; m<Ground_line; m++)//对于每一条扫描线
        {
            if(Last_class[i].detial[m].size() == Last_class[i].Lines_num )
                break;
            for(int n=0; n<Last_class[i].detial[m].size(); n++)//对于每一条扫描线上的 线段
            {
                double x1 = Last_class[i].detial[m][n].Start_point.x, y1 = Last_class[i].detial[m][n].Start_point.y;
                double x2 = Last_class[i].detial[m][n].End_point.x, y2 = Last_class[i].detial[m][n].End_point.y;
                double c = pow(y2-y1,2) + pow(x1-x2,2);
                int h = Last_class[i].detial[m][n].Start_point.SET-35 > 0 ? Last_class[i].detial[m][n].Start_point.SET-35:0;
                int y = Last_class[i].detial[m][n].End_point.SET+35 > (int)laserCloudScansOrder[m].size() ? \
                                                                         (int)laserCloudScansOrder[m].size():Last_class[i].detial[m][n].End_point.SET+35;
                vector<vector<double> >temp_circle_points;
                for(int k=h; k<y; k++)//（）
                {
                    double a = (laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x-x1) * (y2-y1);
                    double b = (laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y-y1) * (x1-x2);
                    double dis = sqrt((a+b)*(a+b)/c);//点到直线的距离
                    if(dis>0.04)
                        continue;
                    double temp_dis1 = pow(Last_class[i].detial[m][n].Start_point.y-laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y,2) + \
                                        pow(Last_class[i].detial[m][n].Start_point.x-laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x,2);
                    double temp_dis2 = pow(Last_class[i].detial[m][n].End_point.y-laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y,2) + \
                                        pow(Last_class[i].detial[m][n].End_point.x-laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x,2);
                    if(temp_dis1>0.81 && temp_dis2>0.81 && (k>Last_class[i].detial[m][n].End_point.SET || k<Last_class[i].detial[m][n].Start_point.SET))
                        continue;
                    point.x = laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].x;
                    point.y = laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].y;
                    point.z = laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].z;
                    point.intensity = laserCloudScansOrder[m][laserCloudScansOrder[m].size()-k-1].intensity;
                    Line_ptr->push_back(point);
                    //vector<double> temp;
                    //temp.push_back(point.x);
                    //temp.push_back(point.y);
                    //temp_circle_points.push_back(temp);
                } 
               // printf("id: %d  %d  %d  ",m,n,(int)Last_class[i].detial[m].size());
               // Fit_Circle(temp_circle_points);
            }
        }
    }
    //K_lines_pub viwer in rviz 
    /*for(int i=0; i<K_means; i++)
    {
        if(Last_class[i].Lines_num <= 1) 
            continue;
        //printf("Class:  %d\n",Last_class[i].Class);/////////////////////////////////////////////////////////
        //printf("Lines_num:  %d\n",Last_class[i].Lines_num);/////////////////////////////////////////////////////////
        for(int j=0; j<Ground_line; j++)
        {
            //printf("    Line_num:  %d\n",(int)Last_class[i].detial[j].size());/////////////////////////////////////////////////////////
            for(int k=0;k<Last_class[i].detial[j].size(); k++)
            {
                geometry_msgs::Point p;
                p.x = Last_class[i].detial[j][k].Start_point.x;
                p.y = Last_class[i].detial[j][k].Start_point.y;
                p.z = 0.1;
                l_marker.points.push_back(p);
                p.x = Last_class[i].detial[j][k].End_point.x;
                p.y = Last_class[i].detial[j][k].End_point.y;
                p.z = 0.2;
                l_marker.points.push_back(p);
                /////////////////////////////////////////////////////////
                //printf("        Id_16lines:  %d\n",Last_class[i].detial[j][k].Id_16lines);
                //printf("        Id_alllines:  %d\n",Last_class[i].detial[j][k].Id_alllines);
                //printf("        K_b:  %f    %f\n",Last_class[i].detial[j][k].K_k,Last_class[i].detial[j][k].K_b);            
                //printf("        Start_point_set_x_y:  %d  %f  %f\n",Last_class[i].detial[j][k].Start_point.SET,Last_class[i].detial[j][k].Start_point.x, Last_class[i].detial[j][k].Start_point.y);       
                //printf("        End_point_set_x_y:    %d  %f  %f\n",Last_class[i].detial[j][k].End_point.SET,Last_class[i].detial[j][k].End_point.x, Last_class[i].detial[j][k].End_point.y);       
                /////////////////////////////////////////////////////////
            }
        }
        //printf("\n\n\n");
    }*/
    //K_lines_pub.publish(l_marker);
    //Access4_class.publish(line_list);

    //发布障碍物2
     Line_ptr->header.frame_id = "velodyne";
     sensor_msgs::PointCloud2 cloud_msg;
     pcl::toROSMsg(*Line_ptr, cloud_msg);
     Access2_pub.publish(cloud_msg);
     *Obs_pointcloud = *Line_ptr;//总障碍物

}

//根据重叠程度检测非可通行区域
//独立函数
//输入原始点云数据
//输出点云分割数据（这里直接显示，不返回）
//参数 2000
void Accessible_part1(int Ground_line,std::vector<pcl::PointCloud<pcl::PointXYZI> > laserCloudScans) //原始点云
{
    //ROS_INFO("Accessible_part1");
    //输入点云、输出障碍点云
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr Obs_ptr (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr New_ptr (new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointXYZI point;
    
    //横向分析
    
    for(int i=0; i<16; i++)
    {

        for(int j=1; j<laserCloudScans[i].size()-1; j++)
        {
            if(laserCloudScans[i][j].x==0 && laserCloudScans[i][j].y==0)
                continue;
            if(laserCloudScans[i][j].z>HIGHEST_OBS || laserCloudScans[i][j+1].z>HIGHEST_OBS)
                continue;
            
            double Dxy =      pow(laserCloudScans[i][j].x - laserCloudScans[i][j+1].x ,2) + \
                              pow(laserCloudScans[i][j].y - laserCloudScans[i][j+1].y, 2) + \
                              pow(laserCloudScans[i][j].z - laserCloudScans[i][j+1].z, 2);//角度小的
            if(Dxy>0.3)
            {
                if(pow(laserCloudScans[i][j].x ,2) + pow(laserCloudScans[i][j].y, 2) < pow(laserCloudScans[i][j+1].x ,2) + pow(laserCloudScans[i][j+1].y, 2))
                {//逻辑有点问题
                    point.x = laserCloudScans[i][j].x;
                    point.y = laserCloudScans[i][j].y;
                    point.z = laserCloudScans[i][j].z;
                }
                else
                {
                    point.x = laserCloudScans[i][j+1].x;
                    point.y = laserCloudScans[i][j+1].y;
                    point.z = laserCloudScans[i][j+1].z;
                }
                Obs_ptr->push_back(point);
            }
        }
        if(i<4)
            continue;
        for(int j=0; j<laserCloudScans[i].size()-1; j++)
        {

            if(laserCloudScans[i][j+1].intensity -laserCloudScans[i][j].intensity>1e-04)//起始点应该是最小的
                continue;          
            if(laserCloudScans[i][j].z>HIGHEST_OBS || laserCloudScans[i][j+1].z>HIGHEST_OBS)
                continue;
            
            double Dxy = sqrt(pow(laserCloudScans[i][j].x-laserCloudScans[i][j+1].x ,2) + pow(laserCloudScans[i][j].y - laserCloudScans[i][j+1].y,2));//角度小的
            double Dz = laserCloudScans[i][j+1].z - laserCloudScans[i][j].z;//顺时针 - 逆时针
            double temp_K = Dz/Dxy;///std::atan2(Dz, Dxy) * (180/M_PI);
            if(fabs(temp_K)>ARCTAN80)//角度差极大，两个都是障碍物//80
            {   
                point.x = laserCloudScans[i][j].x;
                point.y = laserCloudScans[i][j].y;
                point.z = laserCloudScans[i][j].z;
                //point.intensity = seg2000[i][n].intensity;
                Obs_ptr->push_back(point);
                point.x = laserCloudScans[i][j+1].x;
                point.y = laserCloudScans[i][j+1].y;
                point.z = laserCloudScans[i][j+1].z;
                //point.intensity = seg2000[i][m].intensity;
                Obs_ptr->push_back(point);
            }//总障碍物
            else if(fabs(temp_K)>ARCTAN18)//车库建设标准，居民区道路建设标准，都不大于15°//18
            {  
                if(Dz>0)//角度大的的高度高
                {
                    point.x = laserCloudScans[i][j+1].x;
                    point.y = laserCloudScans[i][j+1].y;
                    point.z = laserCloudScans[i][j+1].z;
                    //point.intensity = seg2000[i][n].intensity;
                    Obs_ptr->push_back(point);
                }
                else//角度小的点高度大
                {
                    point.x = laserCloudScans[i][j].x;
                    point.y = laserCloudScans[i][j].y;
                    point.z = laserCloudScans[i][j].z;
                    //point.intensity = seg2000[i][n].intensity;
                    Obs_ptr->push_back(point);
                }

            }
        }

    }
    for(int i=0; i<16; i++)
    {
        *point_cloud += laserCloudScans[i];//0-8 scans
    }
/*
    //相交？
    for(int i=0; i<point_cloud->points.size(); i++)
    {
        for(int j=0; j<ext_points.size(); j=j+2)
        {
            Point A,B;
            A.x = 0;//总障碍物
            A.y = 0;
            B.x = point_cloud->points[i].x;
            B.y = point_cloud->points[i].y; 
            if(intersect(A, B, ext_points[j], ext_points[j+1]))
            {
                point.x = point_cloud->points[i].x;
                point.y = point_cloud->points[i].y;
                point.z = point_cloud->points[i].z;
                //point_cloud->points[i].intensity = -1;
                Obs_ptr->push_back(point);
            }
        }
    }*/
/*
    for(int m=-1500; m<1500; m++)
        for(int n=-1500; n<1500; n++)
            for(int j=0; j<ext_points.size(); j=j+2)
            {
                Point A,B;
                A.x = 0;
                A.y = 0;
                B.x = m/100.0;
                B.y = n/100.0;
                if(intersect(A, B, ext_points[j], ext_points[j+1]))
                {
                    point.x = B.x;
                    point.y = B.y;
                    New_ptr->push_back(point);
                }
            }
*///总障碍物

    for(int i=0; i<ext_points.size(); i=i+2)
    {
        double x1, y1, x2, y2;
        x1 = ext_points[i].x;
        y1 = ext_points[i].y;
        x2 = ext_points[i+1].x;
        y2 = ext_points[i+1].y;
        pcl::PointXYZI point;
        point.x = x1;
        point.y = y1;
        Obs_ptr->push_back(point);
        point.x = x2;
        point.y = y2;
        Obs_ptr->push_back(point);

        double dx = (x2-x1)/1000.0;
        double dy = (y2-y1)/1000.0;//总障碍物
        for(int j=0;j<1000;j++)
        {
            point.x = x1 + dx*j;
            point.y = y1 + dy*j; 
            point.z = -0.2; 
            Obs_ptr->push_back(point);
        }
    }
    ext_points.clear();



    //将点云分割成2000份，可能有的里面没有点
    std::vector<std::vector<Point_cloud> > seg2000;
    seg2000.resize(2000,std::vector<Point_cloud>(0));//2000列，0行
    double Points_AG=0;
    int Points_ID = 0;
    Point_cloud temp_point;
    //ROS_INFO("S1");
    //printf("points.size:  %d\n",(int)point_cloud->points.size());

     for(int i=0; i<point_cloud->points.size(); i++)
    {
       //if(point_cloud->points[i].intensity == -1)
       //     continue;
       Points_AG = point_cloud->points[i].intensity - floor(point_cloud->points[i].intensity);
       //1.单位化 0-1999
       //2.重复点  0 1 1 3？？？之后处理
       Points_ID = round_double(Points_AG*20000)*0.9995;//round_double(Points_AG*10.0*360.0/0.18) * 1999.0/2000.0;
       if(Points_ID>2000.0)
            continue;
       temp_point.x = point_cloud->points[i].x;
       temp_point.y = point_cloud->points[i].y;
       temp_point.z = point_cloud->points[i].z;
       temp_point.intensity = point_cloud->points[i].intensity; 
       seg2000[Points_ID].push_back(temp_point);
    }
    //ROS_INFO("S");
    

    for(int i=0; i<2000; i++)
    {
        //对于每一条射线上 距离很近的点都设置为障碍物
       for(int m=0; m<seg2000[i].size(); m++)
        {
            double temp_dis = pow(seg2000[i][m].x,2) + pow(seg2000[i][m].y,2);
            if(temp_dis>900) //前后左右30m平方
                 continue;
            for(int n=m+1; n<seg2000[i].size(); n++)
            {
                if(n-m > 4)
                    continue;           
                if((0 == floor(seg2000[i][m].intensity)-floor(seg2000[i][n].intensity)))//确保相邻点在同一条Ray上
                    continue;
                double temp_dis = pow(seg2000[i][n].x,2) + pow(seg2000[i][n].y,2);
                if(temp_dis>900) //前后左右30m，平方
                    continue;
                if(seg2000[i][m].z>HIGHEST_OBS || seg2000[i][n].z>HIGHEST_OBS)
                    continue;

                //同一射线上的两个点到原点的距离
                double temp_10 = sqrt(pow(seg2000[i][m].x,2) + pow(seg2000[i][m].y,2));//近点//地面下面可能是空的
                double temp_20 = sqrt(pow(seg2000[i][n].x,2) + pow(seg2000[i][n].y,2));//远点
                if(fabs(temp_20 - temp_10) < 0.05)//近距离点都是障碍物
                {
                    point.x = seg2000[i][n].x;
                    point.y = seg2000[i][n].y;
                    point.z = seg2000[i][n].z;
                    //point.intensity = seg2000[i][n].intensity;
                    Obs_ptr->push_back(point);
                    point.x = seg2000[i][m].x;
                    point.y = seg2000[i][m].y;
                    point.z = seg2000[i][m].z;
                    //point.intensity = seg2000[i][m].intensity;
                    Obs_ptr->push_back(point);
                }
                else if(temp_20 < temp_10)//远点是障碍物
                {
                    point.x = seg2000[i][n].x;
                    point.y = seg2000[i][n].y;
                    point.z = seg2000[i][n].z;
                    //point.intensity = seg2000[i][n].intensity;
                    Obs_ptr->push_back(point);
                }
                else if(temp_20 > temp_10)//远点是障碍物
                {
                    double Dz = seg2000[i][n].z - seg2000[i][m].z;//远点 - 近点    结果可正可负数
                    double Dxy = temp_20 - temp_10;               //永远正数
                    double temp_K = Dz/Dxy;///std::atan2(Dz, Dxy) * (180/M_PI);
                    if(fabs(temp_K)>ARCTAN80)//角度差极大，两个都是障碍物//80
                    {
                        point.x = seg2000[i][n].x;
                        point.y = seg2000[i][n].y;
                        point.z = seg2000[i][n].z;
                        //point.intensity = seg2000[i][n].intensit//总障碍物y;
                        Obs_ptr->push_back(point);
                        point.x = seg2000[i][m].x;
                        point.y = seg2000[i][m].y;
                        point.z = seg2000[i][m].z;
                        //point.intensity = seg2000[i][m].intensity;
                        Obs_ptr->push_back(point);
                    }
                    else if(temp_K>0.28)//车库建设标准，居民区道路建设标准，都不大于15°//18/0.3249
                    {
                        point.x = seg2000[i][n].x;
                        point.y = seg2000[i][n].y;
                        point.z = seg2000[i][n].z;
                        //point.intensity = seg2000[i][n].intensity;
                        Obs_ptr->push_back(point);
                    }
                    else if(temp_K<-0.15)//车库建设标准，居民区道路建设标准，都不大于15°//18/0.3249
                    {
                        point.x = seg2000[i][m].x;
                        point.y = seg2000[i][m].y;
                        point.z = seg2000[i][m].z;
                        //point.intensity = seg2000[i][n].intensity;
                        Obs_ptr->push_back(point);
                    }
                }
            }  
        }  
    }
    //发布障碍区1
    //ROS_INFO("Accessible_part1 over");
    Obs_ptr->header.frame_id = "velodyne";
    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(*Obs_ptr, cloud_msg);
    Access1_pub.publish(cloud_msg);
    *Obs_pointcloud = *Obs_pointcloud + *Obs_ptr;//总障碍物
/*
    nav_msgs::OccupancyGrid map;
    map.header.frame_id="/velodyne";
    map.header.stamp = ros::Time::now(); 
    map.info.origin.position.x=-15;       // float32
    map.info.origin.position.y=-15;       // float32
    map.info.resolution = 0.1;         // float32
    map.info.width      = 300;           // uint32
    map.info.height     = 300;           // uint32
    map.data.resize(map.info.width * map.info.height);

    *Obs_ptr = *Obs_ptr +  *New_ptr;
    for(int i=0; i<Obs_ptr->size(); i++)
    {
        double y = Obs_ptr->points[i].y;//1m
        double x = Obs_ptr->points[i].x;
        int y_cell = y*10;
        int x_cell = x*10;
        if(fabs(y_cell)>=150 || fabs(x_cell)>=150)
            continue;
        map.data[ 150* map.info.width + 150 + y_cell * map.info.width  + x_cell] = -1;

    }
    Access5_grid.publish(map);*/
}




/*************************************************************************
 最小二乘法拟合直线，y = a*x + b; n组数据; r-相关系数[-1,1],fabs(r)->1,说明x,y之间线性关系好，fabs(r)->0，x,y之间无线性关系，拟合无意义
 a = (n*C - B*D) / (n*A - B*B)
 b = (A*D - B*C) / (n*A - B*B)
 r = E / F
 其中：
 A = sum(Xi * Xi)
 B = sum(Xi)
 C = sum(Xi * Yi)
 D = sum(Yi)
 E = sum((Xi - Xmean)*(Yi - Ymean))
 F = sqrt(sum((Xi - Xmean)*(Xi - Xmean))) * sqrt(sum((Yi - Ymean)*(Yi - Ymean)))
 https://blog.csdn.net/pl20140910/article/details/51926886
**************************************************************************/
void LineFitLeastSquares(std::vector<int> index_points, std::vector<std::vector<double> >points, vector<double> &return_KB)
{
	double A = 0.0;
	double B = 0.0;
	double C = 0.0;
	double D = 0.0;
	double E = 0.0;
	double F = 0.0;
 #pragma omp for
	for (int i=0; i<index_points.size(); i++)
	{
		A += points[index_points[i]][1] * points[index_points[i]][1];//y数据   当成x坐标
		B += points[index_points[i]][1];
		C += points[index_points[i]][1] * points[index_points[i]][0];//x数据   当成y坐标
		D += points[index_points[i]][0];
	}
 
	// 计算相关系数r
	double Xmean = B / index_points.size();
	double Ymean = D / index_points.size();

	// 计算斜率a和截距b
	double a, b, temp = 0;
	if( temp = (index_points.size()*A - B*B) )// 判断分母不为0
	{
		a = (index_points.size()*C - B*D) / temp;
		//b = (A*D - B*C) / temp;
	}
	else
	{
		a = 9999;
		//b = Xmean;
	}

	double tempSumXX = 0.0, tempSumYY = 0.0;
	for (int i=0; i<index_points.size(); i++)
	{
		tempSumXX += (points[index_points[i]][1] - Xmean) * (points[index_points[i]][1] - Xmean);
		tempSumYY += (points[index_points[i]][0] - Ymean) * (points[index_points[i]][0] - Ymean);
		E += (points[index_points[i]][1] - Xmean) * (points[index_points[i]][0] - Ymean);
	}
//相关系数
	//F = sqrt(tempSumXX) * sqrt(tempSumYY);
    //double r = E / F;
    //printf("LineFitLeastSquares r: %f   \n",abs(r));
 //   if(fabs(r)<0.6) //(全部换成参数)
 //       return 0;

//K
    //printf("LineFitLeastSquares k: %f   \n",std::atan(a)*(180/M_PI));
    //if(fabs(std::atan(a)*(180/M_PI)-temp_K) > thrd_K)
    //    return 0;
    return_KB.push_back(std::atan(a)*(180/M_PI));
    //*return_K = std::atan(a)*(180/M_PI);
//B
//函数：x = Ky + B  其中 K = d1x/d1y; B = x1-Ky1;  当x=0时的 截距 b = -b/k = y1 - x1/K
    double temp_b;
    if(fabs(a)>1000)//垂直
    {
       temp_b = Xmean;
    }
    else if(a == 0)//180°/0  不能确定
    {
        temp_b = 100;//直接丢弃了
    }
    else 
    {
        temp_b = Xmean - Ymean/a;
        if(temp_b>36) temp_b = 36;
        if(temp_b<-36) temp_b = -36;
    }
    return_KB.push_back(temp_b);

    //printf("LineFitLeastSquares b: %f   \n",temp_b);
    //if(fabs(temp_b-temp_B)>thrd_B)
    //    return 0;
    //return 1;
}


//输入点云m，行起始点s,终止点e
//返回半径
double Fit_Circle(vector<vector<double> >temp_circle_points)
{
    double X1=0,Y1=0,X2=0,Y2=0,X3=0,Y3=0,X1Y1=0,X1Y2=0,X2Y1=0;

    int N = temp_circle_points.size();
    for (int i=0; i<N; i++) 
    {
        double x = temp_circle_points[i][0];
        double y = temp_circle_points[i][1];
        X1 = X1 + x;
        Y1 = Y1 + y;
        X2 = X2 + x*x;
        Y2 = Y2 + y*y;
        X3 = X3 + x*x*x;
        Y3 = Y3 + y*y*y;
        X1Y1 = X1Y1 + x*y;
        X1Y2 = X1Y2 + x*y*y;
        X2Y1 = X2Y1 + x*x*y;
    }
    double a=0,b=0,c=0,C1=0,D1=0,E1=0,G1=0,H1=0;
    C1 = N * X2 - X1 * X1;
    D1 = N * X1Y1 - X1 * Y1;
    E1 = N * X3 + N * X1Y2 - (X2+Y2) * X1;
    G1 = N * Y2 - Y1 * Y1;
    H1 = N * X2Y1 + N * Y3 - (X2+Y2) * Y1;

    double temp = C1 * G1 - D1 * D1;

    if(fabs(temp)<1e-6)
    {
        return 100.0;
    }
    a = (H1*D1 - E1*G1)/(temp);
    b = -(H1*C1 - E1*D1)/(temp);
    c = -(a*X1 + b*Y1 + X2 + Y2)/N;
    //printf("RRRER     %f\n", sqrt(a*a + b*b - 4*c)/2);/////////////////////////////////////////////////////////
    return sqrt(a*a + b*b - 4*c)/2;
}


//直线段相交

double cross(Point A, Point B, Point C)  //AB.AC
{
    return(B.x - A.x) * (C.y - A.y) - (C.x - A.x) * (B.y - A.y);
}

//判段AB和CD是否相交，相交返回 真
#define Min(a,b) (a>b?b:a)
#define Max(a,b) (a>b?a:b)
bool intersect(Point A, Point B, Point C, Point D)
{
    if (Min(A.y, B.y) <= Max(C.y, D.y) &&
        Min(C.y, D.y) <= Max(A.y, B.y) &&
        Min(A.x, B.x) <= Max(C.x, D.x) &&
        Min(C.x, D.x) <= Max(A.x, B.x) &&
        cross(A, B, C) * cross(A, B, D) <= 0 &&
        cross(C, D, A) * cross(C, D, B) <= 0)
        return true;
    return false;
}


